// Equality comparision of floating point values.
//
// Copyright (c) Eric Nodwell
// See LICENSE for details.
//
// Since floating point values are approximate, equality comparison should be
// done to within some limited precision.  In some cases, the best measure of
// precision is a certain number of "ulps" ("Units in Last Place"), where ulps
// essentially is a count of how many representable floating point numbers
// separate two given values.  This is what these functions do.
//
// It is worth emphasizing that a maximum difference of ulps is often not
// the best equality comparison.  However you typically require specific
// domain knowledge of expected precision or error in order to do better.  In
// cases where you don't know or don't care particularly about the expected
// precision, ULPs is typically the best default fall-back.
//
// Basic usage:
//
//   Step 1. (Optional but highly recommended.)
//   Call bonelabMisc::SanityCheck() somewhere in your code.
//
//   Step 2.
//   Compare floating point values (floats and/or doubles), like this:
//
//     if (bonelabMisc::EssentiallyEqual(x,y))
//
//   or
//
//     if (bonelabMisc::ApproximatelyEqual(x,y))
//
//  The former checks that two floating point numbers are within 1 ULP
//  of each other, while the latter has a looser tolerance equal to
//  APPROXIMATELY_EQUAL_ULPS .  I have set APPROXIMATELY_EQUAL_ULPS to 16,
//  but you may tweak it if you want something else.
//
//  As an additional note, one should never use the == operator to compare
//  a float with a double.  The comparsion mostly returns false because
//  following the standard rules of C++, the float is promoted to a double
//  before == is applied.  Thus, you are effectively trying to compare more
//  digits than the number actually has.  The templated comparison functions
//  here by contrast, will demote the double to float if passed both a float
//  and a double.  This is what you want: a meaningful comparison can only be
//  made with the precision of the least precise input.
//
//  This algorithm is a modified verion of the one from:
//    http://www.cygnus-software.com/papers/comparingfloats/comparingfloats.htm
//
//  Boost also has a floating-point comparison module.  It is more appropriate
//  in cases where one can make an estimate for the uncertainty epsilon:
//    http://www.boost.org/doc/libs/1_34_0/libs/test/doc/components/test_tools/floating_point_comparison.html
//
//  For even more information on floating point numbers, refer to
//    http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.22.6768

#include <boost/static_assert.hpp>
#include <boost/math/special_functions/fpclassify.hpp>  // for isnan
#include <boost/cstdint.hpp>
#include <limits>
#include <cstdlib>

// Uncomment this line to get very verbose reporting of comparison.
//#define TRACE_ALMOSTEQUAL2SCOMPLEMENT

namespace bonelabMisc
{

using boost::int64_t;

// Change this value to
const int APPROXIMATELY_EQUAL_ULPS = 16;

// Checks we can do at compile time.
// Please ensure that you also call SanityCheck() in your code.
//
// Note: Hypothetically, one could write the functions below as macros
//       which could then be called as static asserts, thus permitting
//       the whole of SanityCheck to be implemented as static asserts.
//       Then, since inline functions are much preferable to macros
//       for the user, the functions would all wrap the corresponding macro.
//       For now, this is too much work.
BOOST_STATIC_ASSERT(sizeof(int) == sizeof(float));
BOOST_STATIC_ASSERT(sizeof(int64_t) == sizeof(double));

// Description:
// Explicit reinterpretation of float as a 32 bit integer.
// Key point: The value of the int is meaningless, but order is preserved.
inline int FloatTwosComplementUlps (float x)
{
  int xInt = *(int*)&x;
  // Make aInt lexicographically ordered as a twos-complement int
  if (xInt < 0)
    xInt = 0x80000000 - xInt;
  return xInt;
}

// Description:
// Explicit reinterpretation of double as a 64 bit integer
// Key point: The value of the int is meaningless, but order is preserved.
inline int64_t DoubleTwosComplementUlps (double x)
{
  int64_t xInt = *(int64_t*)&x;
  // Make aInt lexicographically ordered as a twos-complement int
  if (xInt < 0)
    xInt = 0x8000000000000000 - xInt;
  return xInt;
}

// Description:
// Given a certain specified precision, return the corresponding number of ulps.
// Note that the correspondence only holds approximately, and is wildly
// inaccurate for denormalized numbers.
inline int FloatUlpsFromPrecision (float p)
{
  float x = 1.0f;
  int ulps = FloatTwosComplementUlps(x+p) - FloatTwosComplementUlps(x);
  return ulps;
}

// Description:
// Given a certain specified precision, return the corresponding number of ulps.
// Note that the correspondence only holds approximately, and is wildly
// inaccurate for denormalized numbers.
inline int64_t DoubleUlpsFromPrecision (double p)
{
  double x = 1.0;
  int64_t ulps = DoubleTwosComplementUlps(x+p) - DoubleTwosComplementUlps(x);
  return ulps;
}

// Description:
// Given the specified number of ulps, return the approximate corresponding
// relative precision.
// Note: Not valid for denormalized numbers.
inline float PrecisionOfFloatUlps (int ulps=1)
{
  float x = 1.0f;
  int xInt = FloatTwosComplementUlps(x);
  int yInt = xInt + ulps;
  float y = *(float*)&yInt;
  return y-x;
}

// Description:
// Given the specified number of ulps, return the approximate corresponding
// relative precision.
// Note: Not valid for denormalized numbers.
inline double PrecisionOfDoubleUlps (int ulps=1)
{
  double x = 1.0f;
  int64_t xInt = DoubleTwosComplementUlps(x);
  int64_t yInt = xInt + ulps;
  double y = *(double*)&yInt;
  return y-x;
}

// Description:
// Compare two floats and return true if they are within a certain number of
// ULPS of each other.
inline bool FloatAlmostEqual2sComplement (float a, float b, int maxUlps=1)
{
    int aInt = FloatTwosComplementUlps(a);
    int bInt = FloatTwosComplementUlps(b);
    int intDiff = std::abs(aInt - bInt);
#ifdef TRACE_ALMOSTEQUAL2SCOMPLEMENT
    cout << "\n";
    cout << std::dec << "a = " << a << "\n";
    cout << std::dec << "b = " << b << "\n";
    cout << std::dec << "*(int*)&a = " << *(int*)&a << " " << std::hex << *(int*)&a << "\n";
    cout << std::dec << "*(int*)&b = " << *(int*)&b << " " << std::hex << *(int*)&b << "\n";
    cout << std::dec << "aInt = " << aInt << " " << std::hex << aInt << "\n";
    cout << std::dec << "bInt = " << bInt << " " << std::hex << bInt << "\n";
    cout << std::dec << "abs(aInt - BInt) = " << intDiff << " " << std::hex << intDiff << "\n";
    cout << std::dec << "maxUlps = " << maxUlps << "\n";
#endif
    // Note that the correct answer to NaN == NaN is false; hence the special case.
    if (intDiff <= maxUlps && !(boost::math::isnan)(a))
        return true;
    return false;
}

// Description:
// Compare two doubles and return true if they are within a certain number of
// ULPS of each other.
inline bool DoubleAlmostEqual2sComplement (double a, double b, int64_t maxUlps=1)
{
    int64_t aInt = DoubleTwosComplementUlps(a);
    int64_t bInt = DoubleTwosComplementUlps(b);
#ifdef WIN32
    // 64 bit Windows has no abs function for 64 bit integers.
    int64_t signedIntDiff = aInt - bInt;
    int64_t intDiff = (signedIntDiff >= 0) ? signedIntDiff : -signedIntDiff;
#else
    int64_t intDiff = std::abs(aInt - bInt);
#endif
#ifdef TRACE_ALMOSTEQUAL2SCOMPLEMENT
    cout << "\n";
    cout << std::dec << "a = " << a << "\n";
    cout << std::dec << "b = " << b << "\n";
    cout << std::dec << "*(int64_t*)&a = " << *(int64_t*)&a << " " << std::hex << *(int64_t*)&a << "\n";
    cout << std::dec << "*(int64_t*)&b = " << *(int64_t*)&b << " " << std::hex << *(int64_t*)&b << "\n";
    cout << std::dec << "aInt = " << aInt << " " << std::hex << aInt << "\n";
    cout << std::dec << "bInt = " << bInt << " " << std::hex << bInt << "\n";
    cout << std::dec << "abs(aInt - BInt) = " << intDiff << " " << std::hex << intDiff << "\n";
    cout << std::dec << "maxUlps = " << maxUlps << "\n";
#endif
    // Note that the correct answer to NaN == NaN is false; hence the special case.
    if (intDiff <= maxUlps && !(boost::math::isnan)(a))
        return true;
    return false;
}

// --------------------------------------------------------------------------
// Templated versions of comparison functions
//
// Note: Comparison of mixed float and double will default to the float
//       versions of the comparison functions.  This is generally sensible.

template <typename T1, typename T2> inline bool AlmostEqual2sComplement (T1 a, T2 b, int maxUlps=1);

template <> inline bool AlmostEqual2sComplement<float, float> (float a, float b, int maxUlps)
{return FloatAlmostEqual2sComplement(a,b,maxUlps);}

template <> inline bool AlmostEqual2sComplement<double, double> (double a, double b, int maxUlps)
{return DoubleAlmostEqual2sComplement(a,b,maxUlps);}

template <> inline bool AlmostEqual2sComplement<float, double> (float a, double b, int maxUlps)
{return FloatAlmostEqual2sComplement(a,(float)b,maxUlps);}

template <> inline bool AlmostEqual2sComplement<double, float> (double a, float b, int maxUlps)
{return FloatAlmostEqual2sComplement((float)a,b,maxUlps);}

// Description:
// Returns true if the arguments are within 1 ULP of each other.
template <typename T1, typename T2> inline bool EssentiallyEqual (T1 a, T2 b)
{return AlmostEqual2sComplement<T1,T2>(a,b,1);}

// Description:
// Returns true if the arguments are within APPROXIMATELY_EQUAL_ULPS of each other.
template <typename T1, typename T2> inline bool ApproximatelyEqual (T1 a, T2 b)
{return AlmostEqual2sComplement<T1,T2>(a,b,APPROXIMATELY_EQUAL_ULPS);}

// Description:
// Checks that the comparison functions work as expected.
// I STRONGLY recommend that you call this somewhere in your code before using
// and of the comparison functions.
void SanityCheck ()
{
  // --------------------------------------------------------------------------
  // Floats

  // Trival check
  assert(!FloatAlmostEqual2sComplement(1.0f,-1.0f,256));
  assert(!FloatAlmostEqual2sComplement(-1.0f,1.0f,256));

  // Positive and Negative zero have different representations, but should
  // compare as equal.
  assert(FloatAlmostEqual2sComplement(0.0f,-0.0f));
  assert(FloatAlmostEqual2sComplement(-0.0f,0.0f));

  // Denormalized minimum value of float should equal 1 when reinterpreted
  // as an int.
  assert(FloatAlmostEqual2sComplement(0.0f,std::numeric_limits<float>::denorm_min()));
  assert(FloatAlmostEqual2sComplement(std::numeric_limits<float>::denorm_min(),0.0f));
  assert(FloatAlmostEqual2sComplement(-0.0f,std::numeric_limits<float>::denorm_min()));
  assert(FloatAlmostEqual2sComplement(std::numeric_limits<float>::denorm_min(),-0.0f));
  assert(FloatAlmostEqual2sComplement(0.0f,-std::numeric_limits<float>::denorm_min()));
  assert(FloatAlmostEqual2sComplement(-std::numeric_limits<float>::denorm_min(),0.0f));
  assert(FloatAlmostEqual2sComplement(-0.0f,-std::numeric_limits<float>::denorm_min()));
  assert(FloatAlmostEqual2sComplement(-std::numeric_limits<float>::denorm_min(),-0.0f));

  // Normalized minimum value of float should be a rather large number when
  // reinterpreted as an int.
  assert(!FloatAlmostEqual2sComplement(0.0f,std::numeric_limits<float>::min()));
  assert(!FloatAlmostEqual2sComplement(std::numeric_limits<float>::min(),0.0f));
  assert(!FloatAlmostEqual2sComplement(-0.0f,std::numeric_limits<float>::min()));
  assert(!FloatAlmostEqual2sComplement(std::numeric_limits<float>::min(),-0.0f));
  assert(!FloatAlmostEqual2sComplement(0.0f,-std::numeric_limits<float>::min()));
  assert(!FloatAlmostEqual2sComplement(-std::numeric_limits<float>::min(),0.0f));
  assert(!FloatAlmostEqual2sComplement(-0.0f,-std::numeric_limits<float>::min()));
  assert(!FloatAlmostEqual2sComplement(-std::numeric_limits<float>::min(),-0.0f));

  // Expected accuracies.
  float lessThanFloatPrecision = PrecisionOfFloatUlps() / 3;
  float moreThanFloatPrecision = PrecisionOfFloatUlps() * 2;
  assert(FloatAlmostEqual2sComplement(1.0f,1.0f+lessThanFloatPrecision));
  assert(FloatAlmostEqual2sComplement(1.0f+lessThanFloatPrecision,1.0f));
  assert(!FloatAlmostEqual2sComplement(1.0f,1.0f+moreThanFloatPrecision));
  assert(!FloatAlmostEqual2sComplement(1.0f+moreThanFloatPrecision,1.0f));
  assert(FloatAlmostEqual2sComplement(-1.0f,-1.0f+lessThanFloatPrecision));
  assert(FloatAlmostEqual2sComplement(-1.0f+lessThanFloatPrecision,-1.0f));
  assert(!FloatAlmostEqual2sComplement(-1.0f,-1.0f+moreThanFloatPrecision));
  assert(!FloatAlmostEqual2sComplement(-1.0f+moreThanFloatPrecision,-1.0f));

  // --------------------------------------------------------------------------
  // Doubles

  // Trival check
  assert(!DoubleAlmostEqual2sComplement(1.0,-1.0,256));
  assert(!DoubleAlmostEqual2sComplement(-1.0,1.0,256));

  // Positive and Negative zero have different representations, but should
  // compare as equal.
  assert(DoubleAlmostEqual2sComplement(0.0,-0.0));
  assert(DoubleAlmostEqual2sComplement(-0.0,0.0));

  // Denormalized minimum value of double should equal 1 when reinterpreted
  // as an int.
  assert(DoubleAlmostEqual2sComplement(0.0,std::numeric_limits<double>::denorm_min()));
  assert(DoubleAlmostEqual2sComplement(std::numeric_limits<double>::denorm_min(),0.0));
  assert(DoubleAlmostEqual2sComplement(-0.0,std::numeric_limits<double>::denorm_min()));
  assert(DoubleAlmostEqual2sComplement(std::numeric_limits<double>::denorm_min(),-0.0));
  assert(DoubleAlmostEqual2sComplement(0.0,-std::numeric_limits<double>::denorm_min()));
  assert(DoubleAlmostEqual2sComplement(-std::numeric_limits<double>::denorm_min(),0.0));
  assert(DoubleAlmostEqual2sComplement(-0.0,-std::numeric_limits<double>::denorm_min()));
  assert(DoubleAlmostEqual2sComplement(-std::numeric_limits<double>::denorm_min(),-0.0));

  // Normalized minimum value of double should be a rather large number when
  // reinterpreted as an int.
  assert(!DoubleAlmostEqual2sComplement(0.0,std::numeric_limits<double>::min()));
  assert(!DoubleAlmostEqual2sComplement(std::numeric_limits<double>::min(),0.0));
  assert(!DoubleAlmostEqual2sComplement(-0.0,std::numeric_limits<double>::min()));
  assert(!DoubleAlmostEqual2sComplement(std::numeric_limits<double>::min(),-0.0));
  assert(!DoubleAlmostEqual2sComplement(0.0,-std::numeric_limits<double>::min()));
  assert(!DoubleAlmostEqual2sComplement(-std::numeric_limits<double>::min(),0.0));
  assert(!DoubleAlmostEqual2sComplement(-0.0,-std::numeric_limits<double>::min()));
  assert(!DoubleAlmostEqual2sComplement(-std::numeric_limits<double>::min(),-0.0));

  // Expected accuracies.
  double lessThanDoublePrecision = PrecisionOfDoubleUlps() / 3;
  double moreThanDoublePrecision = PrecisionOfDoubleUlps() * 2;
  assert(DoubleAlmostEqual2sComplement(1.0,1.0+lessThanDoublePrecision));
  assert(DoubleAlmostEqual2sComplement(1.0+lessThanDoublePrecision,1.0));
  assert(!DoubleAlmostEqual2sComplement(1.0,1.0+moreThanDoublePrecision));
  assert(!DoubleAlmostEqual2sComplement(1.0+moreThanDoublePrecision,1.0));
  assert(DoubleAlmostEqual2sComplement(-1.0,-1.0+lessThanDoublePrecision));
  assert(DoubleAlmostEqual2sComplement(-1.0+lessThanDoublePrecision,-1.0));
  assert(!DoubleAlmostEqual2sComplement(-1.0,-1.0+moreThanDoublePrecision));
  assert(!DoubleAlmostEqual2sComplement(-1.0+moreThanDoublePrecision,-1.0));

  // -------------------------------------------------------------------------
  // EssentiallyEqual and Mixed Floats and Doubles

  // Mixes Zeros (positive and negative)
  assert(EssentiallyEqual(0.0f,0.0));
  assert(EssentiallyEqual(0.0,0.0f));
  assert(EssentiallyEqual(-0.0f,0.0));
  assert(EssentiallyEqual(0.0,-0.0f));
  assert(EssentiallyEqual(0.0f,-0.0));
  assert(EssentiallyEqual(-0.0,0.0f));
  assert(EssentiallyEqual(-0.0f,-0.0));
  assert(EssentiallyEqual(-0.0,-0.0f));

  // Mixed Ones
  assert(EssentiallyEqual(1.0f,1.0));
  assert(EssentiallyEqual(1.0,1.0f));
  assert(EssentiallyEqual(-1.0f,-1.0));
  assert(EssentiallyEqual(-1.0,-1.0f));

  // Expected accuracies should be the same as float, not double
  assert(EssentiallyEqual(1.0f,1.0+lessThanFloatPrecision));
  assert(EssentiallyEqual(1.0+lessThanFloatPrecision,1.0f));
  assert(!EssentiallyEqual(1.0f,1.0+moreThanFloatPrecision));
  assert(!EssentiallyEqual(1.0+moreThanFloatPrecision,1.0f));

  // Finally verify that EssentiallyEqual does give us double accuracies
  // for pure double inputs.
  assert(!EssentiallyEqual(1.0,1.0+moreThanDoublePrecision));
  assert(!EssentiallyEqual(1.0+moreThanDoublePrecision,1.0));

  // -------------------------------------------------------------------------
  // ApproximatelyEqual

  // Float - More tolerant than EssentiallyEqual
  assert(ApproximatelyEqual(1.0f,1.0f+moreThanFloatPrecision));
  assert(ApproximatelyEqual(1.0f+moreThanFloatPrecision,1.0f));

  // Float - Outside specified ULP range for ApproximatelyEqual
  assert(!ApproximatelyEqual(1.0f,1.0f+APPROXIMATELY_EQUAL_ULPS*moreThanFloatPrecision));
  assert(!ApproximatelyEqual(1.0f+APPROXIMATELY_EQUAL_ULPS*moreThanFloatPrecision,1.0f));

  // Double - More tolerant than EssentiallyEqual
  assert(ApproximatelyEqual(1.0,1.0+moreThanDoublePrecision));
  assert(ApproximatelyEqual(1.0+moreThanDoublePrecision,1.0));

  // Double - Outside specified ULP range for ApproximatelyEqual
  assert(!ApproximatelyEqual(1.0,1.0+APPROXIMATELY_EQUAL_ULPS*moreThanDoublePrecision));
  assert(!ApproximatelyEqual(1.0+APPROXIMATELY_EQUAL_ULPS*moreThanDoublePrecision,1.0));

  // -------------------------------------------------------------------------
  // Infinities and NaNs

  // Float infinities
  if (std::numeric_limits<float>::has_infinity)
    {
    assert(!FloatAlmostEqual2sComplement(0.0f,std::numeric_limits<float>::infinity()));
    assert(!FloatAlmostEqual2sComplement(std::numeric_limits<float>::infinity(), 0.0f));
    assert(!FloatAlmostEqual2sComplement(0.0f,-std::numeric_limits<float>::infinity()));
    assert(!FloatAlmostEqual2sComplement(-std::numeric_limits<float>::infinity(), 0.0f));
    assert(!FloatAlmostEqual2sComplement(std::numeric_limits<float>::infinity(),-std::numeric_limits<float>::infinity()));
    assert(!FloatAlmostEqual2sComplement(-std::numeric_limits<float>::infinity(), std::numeric_limits<float>::infinity()));
    assert(FloatAlmostEqual2sComplement(std::numeric_limits<float>::infinity(),std::numeric_limits<float>::infinity()));
    assert(FloatAlmostEqual2sComplement(-std::numeric_limits<float>::infinity(), -std::numeric_limits<float>::infinity()));
    }

  // Double infinities
  if (std::numeric_limits<double>::has_infinity)
    {
    assert(!DoubleAlmostEqual2sComplement(0.0,std::numeric_limits<double>::infinity()));
    assert(!DoubleAlmostEqual2sComplement(std::numeric_limits<double>::infinity(), 0.0));
    assert(!DoubleAlmostEqual2sComplement(0.0,-std::numeric_limits<double>::infinity()));
    assert(!DoubleAlmostEqual2sComplement(-std::numeric_limits<double>::infinity(), 0.0));
    assert(!DoubleAlmostEqual2sComplement(std::numeric_limits<double>::infinity(),-std::numeric_limits<double>::infinity()));
    assert(!DoubleAlmostEqual2sComplement(-std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity()));
    assert(DoubleAlmostEqual2sComplement(std::numeric_limits<double>::infinity(),std::numeric_limits<double>::infinity()));
    assert(DoubleAlmostEqual2sComplement(-std::numeric_limits<double>::infinity(), -std::numeric_limits<double>::infinity()));
    }

  // float NaNs
  if (std::numeric_limits<float>::has_quiet_NaN)
    {
    assert(!FloatAlmostEqual2sComplement(0.0f,std::numeric_limits<float>::quiet_NaN()));
    assert(!FloatAlmostEqual2sComplement(std::numeric_limits<float>::quiet_NaN(),0.0f));
    assert(!FloatAlmostEqual2sComplement(1.0f,std::numeric_limits<float>::quiet_NaN()));
    assert(!FloatAlmostEqual2sComplement(std::numeric_limits<float>::quiet_NaN(),1.0f));
    assert(!FloatAlmostEqual2sComplement(std::numeric_limits<float>::infinity(),std::numeric_limits<float>::quiet_NaN()));
    assert(!FloatAlmostEqual2sComplement(std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::infinity()));
    assert(!FloatAlmostEqual2sComplement(-std::numeric_limits<float>::infinity(),std::numeric_limits<float>::quiet_NaN()));
    assert(!FloatAlmostEqual2sComplement(std::numeric_limits<float>::quiet_NaN(),-std::numeric_limits<float>::infinity()));
    // Note that according to the standard the correct answer to NaN == NaN is false
    assert(!FloatAlmostEqual2sComplement(std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN()));
    }

  // Double NaNs
  if (std::numeric_limits<double>::has_quiet_NaN)
    {
    assert(!DoubleAlmostEqual2sComplement(0.0,std::numeric_limits<double>::quiet_NaN()));
    assert(!DoubleAlmostEqual2sComplement(std::numeric_limits<double>::quiet_NaN(),0.0));
    assert(!DoubleAlmostEqual2sComplement(1.0,std::numeric_limits<double>::quiet_NaN()));
    assert(!DoubleAlmostEqual2sComplement(std::numeric_limits<double>::quiet_NaN(),1.0));
    assert(!DoubleAlmostEqual2sComplement(std::numeric_limits<double>::infinity(),std::numeric_limits<double>::quiet_NaN()));
    assert(!DoubleAlmostEqual2sComplement(std::numeric_limits<double>::quiet_NaN(),std::numeric_limits<double>::infinity()));
    assert(!DoubleAlmostEqual2sComplement(-std::numeric_limits<double>::infinity(),std::numeric_limits<double>::quiet_NaN()));
    assert(!DoubleAlmostEqual2sComplement(std::numeric_limits<double>::quiet_NaN(),-std::numeric_limits<double>::infinity()));
    // Note that according to the standard the correct answer to NaN == NaN is false
    assert(!DoubleAlmostEqual2sComplement(std::numeric_limits<double>::quiet_NaN(),std::numeric_limits<double>::quiet_NaN()));
    }

}

}  // namespace


//
// Created by xiang on 12/21/17.
//

#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;

#include <vector>
#include <fstream>
#include <iostream>
#include <iomanip>

#include "sophus/se3.h"

using namespace std;

typedef vector<Vector3d, Eigen::aligned_allocator<Vector3d>> VecVector3d;
typedef vector<Vector2d, Eigen::aligned_allocator<Vector3d>> VecVector2d;
typedef Matrix<double, 6, 1> Vector6d;

string p3d_file = "./p3d.txt";
string p2d_file = "./p2d.txt";

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

    VecVector2d p2d;
    VecVector3d p3d;
    Matrix3d K;
    double fx = 520.9, fy = 521.0, cx = 325.1, cy = 249.7;
    K << fx, 0, cx, 0, fy, cy, 0, 0, 1;

    // load points in to p3d and p2d
    // START YOUR CODE HERE

    // END YOUR CODE HERE
    assert(p3d.size() == p2d.size());

    int iterations = 100;
    double cost = 0, lastCost = 0;
    int nPoints = p3d.size();
    cout << "points: " << nPoints << endl;

    Sophus::SE3 T_esti; // estimated pose

    for (int iter = 0; iter < iterations; iter++) {

        Matrix<double, 6, 6> H = Matrix<double, 6, 6>::Zero();
        Vector6d b = Vector6d::Zero();

        cost = 0;
        // compute cost
        for (int i = 0; i < nPoints; i++) {
            // compute cost for p3d[I] and p2d[I]
            // START YOUR CODE HERE

	    // END YOUR CODE HERE

	    // compute jacobian
            Matrix<double, 2, 6> J;
            // START YOUR CODE HERE

	    // END YOUR CODE HERE

            H += J.transpose() * J;
            b += -J.transpose() * e;
        }

	// solve dx
        Vector6d dx;

        // START YOUR CODE HERE

        // END YOUR CODE HERE

        if (isnan(dx[0])) {
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost >= lastCost) {
            // cost increase, update is not good
            cout << "cost: " << cost << ", last cost: " << lastCost << endl;
            break;
        }

        // update your estimation
        // START YOUR CODE HERE

        // END YOUR CODE HERE

        lastCost = cost;

        cout << "iteration " << iter << " cost=" << cout.precision(12) << cost << endl;
    }

    cout << "estimated pose: \n" << T_esti.matrix() << endl;
    return 0;
}


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <scitbx/matrix/row_echelon.h>
#include <scitbx/matrix/row_echelon_full_pivoting.h>
#include <scitbx/array_family/versa.h>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>

namespace scitbx { namespace matrix { namespace boost_python {

namespace {

  template <typename ElementType>
  af::ref<ElementType, af::mat_grid>
  flex_as_mat_ref(af::versa<ElementType, af::flex_grid<> >& a)
  {
    SCITBX_ASSERT(a.accessor().nd() == 2);
    SCITBX_ASSERT(a.accessor().is_0_based());
    SCITBX_ASSERT(!a.accessor().is_padded());
    return af::ref<ElementType, af::mat_grid>(
      a.begin(),
      a.accessor().all()[0],
      a.accessor().all()[1]);
  }

  std::size_t
  row_echelon_form_t(
    af::versa<int, af::flex_grid<> >& m,
    af::versa<int, af::flex_grid<> >& t)
  {
    af::ref<int, af::mat_grid> m_ref = flex_as_mat_ref(m);
    af::ref<int, af::mat_grid> t_ref = flex_as_mat_ref(t);
    std::size_t rank = row_echelon::form_t(m_ref, t_ref);
    m.resize(af::flex_grid<>(m_ref.n_rows(), m_ref.n_columns()));
    return rank;
  }

  std::size_t
  row_echelon_form(
    af::versa<int, af::flex_grid<> >& m)
  {
    af::ref<int, af::mat_grid> m_ref = flex_as_mat_ref(m);
    std::size_t rank = row_echelon::form(m_ref);
    m.resize(af::flex_grid<>(m_ref.n_rows(), m_ref.n_columns()));
    return rank;
  }

  int
  row_echelon_back_substitution_int(
    af::versa<int, af::flex_grid<> >& re_mx,
    af::const_ref<int> const& v,
    af::ref<int> const& sol,
    af::ref<bool> const& indep)
  {
    af::ref<int, af::mat_grid> re_mx_ref = flex_as_mat_ref(re_mx);
    const int* v_ptr = 0;
    int* sol_ptr = 0;
    bool* indep_ptr = 0;
    if (v.size()) {
      SCITBX_ASSERT(v.size() == re_mx_ref.n_rows());
      v_ptr = v.begin();
    }
    if (sol.size()) {
      SCITBX_ASSERT(sol.size() == re_mx_ref.n_columns());
      sol_ptr = sol.begin();
    }
    if (indep.size()) {
      SCITBX_ASSERT(indep.size() == re_mx_ref.n_columns());
      indep_ptr = indep.begin();
    }
    return row_echelon::back_substitution_int(
      re_mx_ref, v_ptr, sol_ptr, indep_ptr);
  }

  bool
  row_echelon_back_substitution_float(
    af::versa<int, af::flex_grid<> >& re_mx,
    af::const_ref<double> const& v,
    af::ref<double> const& sol)
  {
    af::ref<int, af::mat_grid> re_mx_ref = flex_as_mat_ref(re_mx);
    const double* v_ptr = 0;
    double* sol_ptr = 0;
    if (v.size()) {
      SCITBX_ASSERT(v.size() == re_mx_ref.n_rows());
      v_ptr = v.begin();
    }
    if (sol.size()) {
      SCITBX_ASSERT(sol.size() == re_mx_ref.n_columns());
      sol_ptr = sol.begin();
    }
    return row_echelon::back_substitution_float(
      re_mx_ref, v_ptr, sol_ptr);
  }

  void wrap_row_echelon()
  {
    using namespace boost::python;
    def("row_echelon_form_t", row_echelon_form_t);
    def("row_echelon_form", row_echelon_form);
    def("row_echelon_back_substitution_int",
      row_echelon_back_substitution_int);
    def("row_echelon_back_substitution_float",
      row_echelon_back_substitution_float);
  }

  struct full_pivoting_wrapper
  {
    typedef row_echelon::full_pivoting<double> wt;

    static void wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<return_by_value> rbv;
      class_<wt>("row_echelon_full_pivoting", no_init)
        .def(init<
          af::versa<double, af::flex_grid<> >,
          double const&,
          int>((
            arg("a_work"),
            arg("min_abs_pivot")=0,
            arg("max_rank")=-1)))
        .def(init<
          af::versa<double, af::flex_grid<> >,
          af::shared<double>,
          double const&,
          int>((
            arg("a_work"),
            arg("b_work"),
            arg("min_abs_pivot")=0,
            arg("max_rank")=-1)))
        .add_property("col_perm", make_getter(&wt::col_perm, rbv()))
        .def_readonly("rank", &wt::rank)
        .def_readonly("nullity", &wt::nullity)
        .def("is_in_row_space", &wt::is_in_row_space, (
          arg("x"),
          arg("epsilon")))
        .def("back_substitution", &wt::back_substitution, (
          arg("free_values"),
          arg("epsilon")=0))
      ;
    }
  };

}}} // namespace matrix::boost_python::<anonymous>

namespace math { namespace boost_python {

  void wrap_row_echelon()
  {
    matrix::boost_python::wrap_row_echelon();
    matrix::boost_python::full_pivoting_wrapper::wrap();
  }

}}} // namespace scitbx::math::boost_python


// Copyright Nick Thompson, 2020
// 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 <iostream>
#include <unordered_map>
#include <string>
#include <future>
#include <thread>
#include <fstream>
#include <boost/hana/for_each.hpp>
#include <boost/hana/ext/std/integer_sequence.hpp>
#include <boost/math/special_functions/daubechies_scaling.hpp>
#include <boost/math/special_functions/detail/daubechies_scaling_integer_grid.hpp>
#include <boost/math/interpolators/cubic_hermite.hpp>
#include <boost/math/interpolators/quintic_hermite.hpp>
#include <boost/math/interpolators/quintic_hermite.hpp>
#include <boost/math/interpolators/septic_hermite.hpp>
#include <boost/math/interpolators/cardinal_quadratic_b_spline.hpp>
#include <boost/math/interpolators/cardinal_cubic_b_spline.hpp>
#include <boost/math/interpolators/cardinal_quintic_b_spline.hpp>
#include <boost/math/interpolators/whittaker_shannon.hpp>
#include <boost/math/interpolators/cardinal_trigonometric.hpp>
#include <boost/math/special_functions/next.hpp>
#include <boost/math/interpolators/makima.hpp>
#include <boost/math/interpolators/pchip.hpp>
#include <boost/multiprecision/float128.hpp>
#include <boost/core/demangle.hpp>

using boost::multiprecision::float128;

template<typename Real, typename PreciseReal, int p>
void choose_refinement()
{
    std::cout << "Choosing refinement for " << boost::core::demangle(typeid(Real).name()) << " precision Daubechies scaling function with " << p << " vanishing moments.\n";
    using std::abs;
    int rmax = 22;
    auto phi_dense = boost::math::daubechies_scaling_dyadic_grid<PreciseReal, p, 0>(rmax);
    Real dx_dense = (2*p-1)/static_cast<Real>(phi_dense.size()-1);

    for (int r = 2; r <= 18; ++r)
    {
        Real dx = Real(1)/ (1 << r);
        std::cout << "\tdx = 1/" << (1/dx) << " = 1/2^" << r << " = " << dx << "\n";
        auto phi = boost::math::daubechies_scaling<Real, p>(r);
        Real max_flt_distance = 0;
        Real sup  = 0;
        Real rel_sup = 0;
        Real worst_flt_abscissa = 0;
        Real worst_flt_value = 0;
        Real worst_flt_computed = 0;

        Real worst_rel_abscissa = 0;
        Real worst_rel_value = 0;
        Real worst_rel_computed = 0;

        Real worst_abs_abscissa = 0;
        Real worst_abs_computed = 0;
        Real worst_abs_expected = 0;
        for (size_t i = 0; i < phi_dense.size(); ++i)
        {
            Real t = i*dx_dense;
            Real computed = phi(t);
            Real expected = Real(phi_dense[i]);
            Real abs_diff = abs(computed - expected);
            Real rel_diff = abs_diff/abs(expected);
            Real flt_distance = abs(boost::math::float_distance(computed, expected));
            if (flt_distance > max_flt_distance)
            {
                max_flt_distance = flt_distance;
                worst_flt_abscissa = t;
                worst_flt_value = expected;
                worst_flt_computed = computed;
            }
            if (expected != 0 && rel_diff > rel_sup)
            {
                rel_sup = rel_diff;
                worst_rel_abscissa = t;
                worst_rel_value = expected;
                worst_rel_computed = computed;

            }
            if (abs_diff > sup)
            {
                sup = abs_diff;
                worst_abs_abscissa = t;
                worst_abs_computed = computed;
                worst_abs_expected = expected;
            }
        }
        std::cout << "\t\tFloat distance at r = " << r << " is " << max_flt_distance << ", sup distance = " << sup << ", max relative error = " << rel_sup << "\n";
        std::cout << "\t\tWorst flt abscissa = " << worst_flt_abscissa << ", worst expected value = " << worst_flt_value << ", computed = " << worst_flt_computed << "\n";
        std::cout << "\t\tWorst rel abscissa = " << worst_rel_abscissa << ", worst expected value = " << worst_rel_value << ", computed = " << worst_rel_computed << "\n";
        std::cout << "\t\tWorst abs abscissa = " << worst_abs_abscissa << ", worst expected value = " << worst_abs_computed << ", worst abs value (expected) = " << worst_abs_expected << "\n";
    }
    std::cout << "\n\n\n";
}

template<typename Real, typename PreciseReal, int p>
void find_best_interpolator()
{
    std::string filename = "daubechies_" + std::to_string(p) + "_scaling_convergence.csv";
    std::ofstream fs{filename};
    static_assert(sizeof(PreciseReal) >= sizeof(Real), "sizeof(PreciseReal) >= sizeof(Real) is required.");
    using std::abs;
    int rmax = 18;
    std::cout << "Computing phi_dense_precise\n";
    auto phi_dense_precise = boost::math::daubechies_scaling_dyadic_grid<PreciseReal, p, 0>(rmax);
    std::vector<Real> phi_dense(phi_dense_precise.size());
    for (size_t i = 0; i < phi_dense.size(); ++i)
    {
        phi_dense[i] = static_cast<Real>(phi_dense_precise[i]);
    }
    phi_dense_precise.resize(0);
    std::cout << "Done\n";

    Real dx_dense = (2*p-1)/static_cast<Real>(phi_dense.size()-1);
    fs << std::setprecision(std::numeric_limits<Real>::digits10 + 3);
    fs << std::fixed;
    fs << "r, matched_holder, linear, quadratic_b_spline, cubic_b_spline, quintic_b_spline, cubic_hermite, pchip, makima, fo_taylor";
    if (p==2)
    {
        fs << "\n";
    }
    else
    {
        fs << ", quintic_hermite, second_order_taylor";
        if (p > 3)
        {
            fs << ", third_order_taylor, septic_hermite\n";
        }
        else
        {
            fs << "\n";
        }
    }
    for (int r = 2; r < 13; ++r)
    {
        fs << r << ", ";
        std::map<Real, std::string> m;
        auto phi = boost::math::daubechies_scaling_dyadic_grid<Real, p, 0>(r);
        auto phi_prime = boost::math::daubechies_scaling_dyadic_grid<Real, p, 1>(r);

        std::vector<Real> x(phi.size());
        Real dx = (2*p-1)/static_cast<Real>(x.size()-1);
        std::cout << "dx = 1/" << (1 << r) << " = " << dx << "\n";
        for (size_t i = 0; i < x.size(); ++i)
        {
            x[i] = i*dx;
        }

        {
            auto phi_copy = phi;
            auto phi_prime_copy = phi_prime;
            auto mh = boost::math::detail::matched_holder(std::move(phi_copy), std::move(phi_prime_copy), r, Real(0));
            Real sup = 0;
            // call to matched_holder is unchecked, so only go to phi_dense.size() -1.
            for (size_t i = 0; i < phi_dense.size() - 1; ++i)
            {
                Real x = i*dx_dense;
                Real diff = abs(phi_dense[i] - mh(x));
                if (diff > sup)
                {
                    sup = diff;
                }
            }
            m.insert({sup, "matched_holder"});
            fs << sup << ", ";
        }

        {
            auto linear = [&phi, &dx, &r](Real x)->Real {
              if (x <= 0 || x >= 2*p-1)
              {
                return Real(0);
              }
              using std::floor;

              Real y = (1<<r)*x;
              Real k = floor(y);

              size_t kk = static_cast<size_t>(k);

              Real t = y - k;
              return (1-t)*phi[kk] + t*phi[kk+1];
            };

            Real linear_sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i)
            {
                Real x = i*dx_dense;
                Real diff = abs(phi_dense[i] - linear(x));
                if (diff > linear_sup)
                {
                    linear_sup = diff;
                }
            }
            m.insert({linear_sup, "linear interpolation"});
            fs << linear_sup << ", ";
        }

        {
            auto qbs = boost::math::interpolators::cardinal_quadratic_b_spline(phi.data(), phi.size(), Real(0), dx, phi_prime.front(), phi_prime.back());
            Real qbs_sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i)
            {
                Real x = i*dx_dense;
                Real diff = abs(phi_dense[i] - qbs(x));
                if (diff > qbs_sup) {
                    qbs_sup = diff;
                }
            }
            m.insert({qbs_sup, "quadratic_b_spline"});
            fs << qbs_sup << ", ";
        }

        {
            auto cbs = boost::math::interpolators::cardinal_cubic_b_spline(phi.data(), phi.size(), Real(0), dx, phi_prime.front(), phi_prime.back());
            Real cbs_sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i)
            {
                Real x = i*dx_dense;
                Real diff = abs(phi_dense[i] - cbs(x));
                if (diff > cbs_sup)
                {
                    cbs_sup = diff;
                }
            }
            m.insert({cbs_sup, "cubic_b_spline"});
            fs << cbs_sup << ", ";
        }

        {
            auto qbs = boost::math::interpolators::cardinal_quintic_b_spline(phi.data(), phi.size(), Real(0), dx, {0,0}, {0,0});
            Real qbs_sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i)
            {
                Real x = i*dx_dense;
                Real diff = abs(phi_dense[i] - qbs(x));
                if (diff > qbs_sup)
                {
                    qbs_sup = diff;
                }
            }
            m.insert({qbs_sup, "quintic_b_spline"});
            fs << qbs_sup << ", ";
        }

        {
            auto phi_copy = phi;
            auto phi_prime_copy = phi_prime;
            auto ch = boost::math::interpolators::cardinal_cubic_hermite(std::move(phi_copy), std::move(phi_prime_copy), Real(0), dx);
            Real chs_sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i)
            {
                Real x = i*dx_dense;
                Real diff = abs(phi_dense[i] - ch(x));
                if (diff > chs_sup)
                {
                    chs_sup = diff;
                }
            }
            m.insert({chs_sup, "cubic_hermite_spline"});
            fs << chs_sup << ", ";
        }

        {
            auto phi_copy = phi;
            auto x_copy = x;
            auto phi_prime_copy = phi_prime;
            auto pc = boost::math::interpolators::pchip(std::move(x_copy), std::move(phi_copy));
            Real pchip_sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i)
            {
                Real x = i*dx_dense;
                Real diff = abs(phi_dense[i] - pc(x));
                if (diff > pchip_sup)
                {
                  pchip_sup = diff;
                }
            }
            m.insert({pchip_sup, "pchip"});
            fs << pchip_sup << ", ";
        }

        {
            auto phi_copy = phi;
            auto x_copy = x;
            auto pc = boost::math::interpolators::makima(std::move(x_copy), std::move(phi_copy));
            Real makima_sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i) {
              Real x = i*dx_dense;
              Real diff = abs(phi_dense[i] - pc(x));
              if (diff > makima_sup)
              {
                  makima_sup = diff;
              }
            }
            m.insert({makima_sup, "makima"});
            fs << makima_sup << ", ";
        }

        // Whittaker-Shannon interpolation has linear complexity; test over all points and it's quadratic.
        // I ran this a couple times and found it's not competitive; so comment out for now.
        /*{
            auto phi_copy = phi;
            auto ws = boost::math::interpolators::whittaker_shannon(std::move(phi_copy), Real(0), dx);
            Real sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i) {
              Real x = i*dx_dense;
              using std::abs;
              Real diff = abs(phi_dense[i] - ws(x));
              if (diff > sup) {
                sup = diff;
              }
            }

            m.insert({sup, "whittaker_shannon"});
        }

        // Again, linear complexity of evaluation => quadratic complexity of exhaustive checking.
        {
            auto trig = boost::math::interpolators::cardinal_trigonometric(phi, Real(0), dx);
            Real sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i) {
              Real x = i*dx_dense;
              using std::abs;
              Real diff = abs(phi_dense[i] - trig(x));
              if (diff > sup) {
                sup = diff;
              }
            }
            m.insert({sup, "trig"});
        }*/

        {
            auto fotaylor = [&phi, &phi_prime, &r](Real x)->Real
            {
                if (x <= 0 || x >= 2*p-1)
                {
                    return 0;
                }
                using std::floor;

                Real y = (1<<r)*x;
                Real k = floor(y);

                size_t kk = static_cast<size_t>(k);
                if (y - k < k + 1 - y)
                {
                    Real eps = (y-k)/(1<<r);
                    return phi[kk] + eps*phi_prime[kk];
                }
                else {
                    Real eps = (y-k-1)/(1<<r);
                    return phi[kk+1] + eps*phi_prime[kk+1];
                }
            };
            Real fo_sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i)
            {
                Real x = i*dx_dense;
                Real diff = abs(phi_dense[i] - fotaylor(x));
                if (diff > fo_sup)
                {
                  fo_sup = diff;
                }
            }
            m.insert({fo_sup, "First-order Taylor"});
            if (p==2)
            {
                fs << fo_sup << "\n";
            }
            else
            {
                fs << fo_sup << ", ";
            }
        }

        if constexpr (p > 2) {
            auto phi_dbl_prime = boost::math::daubechies_scaling_dyadic_grid<Real, p, 2>(r);

            {
                auto phi_copy = phi;
                auto phi_prime_copy = phi_prime;
                auto phi_dbl_prime_copy = phi_dbl_prime;
                auto qh = boost::math::interpolators::cardinal_quintic_hermite(std::move(phi_copy), std::move(phi_prime_copy), std::move(phi_dbl_prime_copy), Real(0), dx);
                Real qh_sup = 0;
                for (size_t i = 0; i < phi_dense.size(); ++i)
                {
                    Real x = i*dx_dense;
                    Real diff = abs(phi_dense[i] - qh(x));
                    if (diff > qh_sup)
                    {
                        qh_sup = diff;
                    }
                }
                m.insert({qh_sup, "quintic_hermite_spline"});
                fs << qh_sup << ", ";
            }

            {
                auto sotaylor = [&phi, &phi_prime, &phi_dbl_prime, &r](Real x)->Real {
                      if (x <= 0 || x >= 2*p-1)
                      {
                          return 0;
                      }
                      using std::floor;

                      Real y = (1<<r)*x;
                      Real k = floor(y);

                      size_t kk = static_cast<size_t>(k);
                      if (y - k < k + 1 - y)
                      {
                          Real eps = (y-k)/(1<<r);
                          return phi[kk] + eps*phi_prime[kk] + eps*eps*phi_dbl_prime[kk]/2;
                      }
                      else {
                          Real eps = (y-k-1)/(1<<r);
                          return phi[kk+1] + eps*phi_prime[kk+1] + eps*eps*phi_dbl_prime[kk+1]/2;
                      }
                };
                Real so_sup = 0;
                for (size_t i = 0; i < phi_dense.size(); ++i)
                {
                    Real x = i*dx_dense;
                    Real diff = abs(phi_dense[i] - sotaylor(x));
                    if (diff > so_sup)
                    {
                        so_sup = diff;
                    }
                }
                m.insert({so_sup, "Second-order Taylor"});
                if (p > 3)
                {
                    fs << so_sup << ", ";
                }
                else
                {
                    fs << so_sup << "\n";
                }

            }
        }

        if constexpr (p > 3)
        {
            auto phi_dbl_prime = boost::math::daubechies_scaling_dyadic_grid<Real, p, 2>(r);
            auto phi_triple_prime = boost::math::daubechies_scaling_dyadic_grid<Real, p, 3>(r);

            {
                auto totaylor = [&phi, &phi_prime, &phi_dbl_prime, &phi_triple_prime, &r](Real x)->Real {
                      if (x <= 0 || x >= 2*p-1) {
                          return 0;
                      }
                      using std::floor;

                      Real y = (1<<r)*x;
                      Real k = floor(y);

                      size_t kk = static_cast<size_t>(k);
                      if (y - k < k + 1 - y)
                      {
                          Real eps = (y-k)/(1<<r);
                          return phi[kk] + eps*phi_prime[kk] + eps*eps*phi_dbl_prime[kk]/2 + eps*eps*eps*phi_triple_prime[kk]/6;
                      }
                      else {
                          Real eps = (y-k-1)/(1<<r);
                          return phi[kk+1] + eps*phi_prime[kk+1] + eps*eps*phi_dbl_prime[kk+1]/2 + eps*eps*eps*phi_triple_prime[kk]/6;
                      }
                };
                Real to_sup = 0;
                for (size_t i = 0; i < phi_dense.size(); ++i)
                {
                    Real x = i*dx_dense;
                    Real diff = abs(phi_dense[i] - totaylor(x));
                    if (diff > to_sup)
                    {
                        to_sup = diff;
                    }
                }

                m.insert({to_sup, "Third-order Taylor"});
                fs << to_sup << ", ";
            }

            {
                auto phi_copy = phi;
                auto phi_prime_copy = phi_prime;
                auto phi_dbl_prime_copy = phi_dbl_prime;
                auto phi_triple_prime_copy = phi_triple_prime;
                auto sh = boost::math::interpolators::cardinal_septic_hermite(std::move(phi_copy), std::move(phi_prime_copy), std::move(phi_dbl_prime_copy), std::move(phi_triple_prime_copy), Real(0), dx);
                Real septic_sup = 0;
                for (size_t i = 0; i < phi_dense.size(); ++i)
                {
                    Real x = i*dx_dense;
                    Real diff = abs(phi_dense[i] - sh(x));
                    if (diff > septic_sup)
                    {
                        septic_sup = diff;
                    }
                }
                m.insert({septic_sup, "septic_hermite_spline"});
                fs << septic_sup << "\n";
            }

        }
        std::string best = "none";
        Real best_sup = 1000000000;
        std::cout << std::setprecision(std::numeric_limits<Real>::digits10 + 3) << std::fixed;
        for (auto & e : m)
        {
            std::cout << "\t" << e.first << " is error of " << e.second << "\n";
            if (e.first < best_sup)
            {
                best = e.second;
                best_sup = e.first;
            }
        }
        std::cout << "\tThe best method for p = " << p << " is the " << best << "\n";
    }
}

int main()
{
    //boost::hana::for_each(std::make_index_sequence<4>(), [&](auto i){ choose_refinement<double, float128, i+16>(); });
    boost::hana::for_each(std::make_index_sequence<12>(), [&](auto i){ find_best_interpolator<double, float128, i+2>(); });
}


#include <armadillo>
#include <bitset>
#include <functional>

using namespace arma;
using namespace std;

namespace genetico {

template <unsigned int nBits>
bitset<nBits> codificar(double fenotipo,
                        double minimoFenotipo,
                        double maximoFenotipo)
{
    double rango = maximoFenotipo - minimoFenotipo;
    double factorConversion = (pow(2, nBits) - 1) / rango;

    unsigned int convertido = round((fenotipo - minimoFenotipo) * factorConversion);

    return bitset<nBits>{convertido};
}

template <unsigned int nBits, unsigned int nVariables>
bitset<nBits * nVariables> codificar(array<double, nVariables> fenotipo,
                                     const array<double, nVariables * 2>& limites)
{
    ostringstream ost;
    for (unsigned int i = 0; i < nVariables; ++i) {
        ost << codificar<nBits>(fenotipo.at(i),
                                limites.at(2 * i),
                                limites.at(2 * i + 1));
    }

    bitset<nBits * nVariables> result;
    istringstream ist{ost.str()};
    ist >> result;

    return result;
}

template <unsigned int nBits>
double decodificar(bitset<nBits> genotipo,
                   double minimoFenotipo,
                   double maximoFenotipo)
{
    double rango = maximoFenotipo - minimoFenotipo;
    double factorConversion = rango / (pow(2, nBits) - 1);

    double convertido = genotipo.to_ulong() * factorConversion + minimoFenotipo;

    return convertido;
}

template <unsigned int nBits, unsigned int nVariables>
array<double, nVariables> decodificar(const bitset<nBits * nVariables>& genotipo,
                                      const array<double, nVariables * 2>& limites)
{
    istringstream ist{genotipo.to_string()};
    array<double, nVariables> result;

    for (unsigned int i = 0; i < nVariables; ++i) {
        bitset<nBits> trozoGenotipo;
        ist >> trozoGenotipo;
        result.at(i) = decodificar<nBits>(trozoGenotipo,
                                          limites.at(2 * i),
                                          limites.at(2 * i + 1));
    }

    return result;
}

// Genotipo con un único cromosoma
template <unsigned int nBits, unsigned int nVariables>
struct Individuo {
    // Individuo inicializado al azar
    Individuo(const array<double, nVariables * 2>& t_limites)
        : genotipo{unsigned(randi(1, distr_param(0.0, pow(2, nBits * nVariables) - 1)).at(0))}
        , limites{t_limites}
    {
    }

    Individuo(bitset<nBits * nVariables> t_genotipo,
              const array<double, nVariables * 2>& t_limites)
        : genotipo{t_genotipo}
        , limites{t_limites}
    {
    }

    array<double, nVariables> fenotipo() const
    {
        return decodificar<nBits, nVariables>(genotipo, limites);
    }

    void mutar()
    {
        genotipo.flip(randi(1, distr_param(0.0, nBits * nVariables - 1)).at(0));
    }

    bitset<nBits * nVariables> genotipo;
    array<double, nVariables * 2> limites;
};

template <unsigned int nBits, unsigned int nVariables>
class Poblacion {
    using I = Individuo<nBits, nVariables>;

public:
    Poblacion(const array<double, nVariables * 2>& limites,
              const std::function<double(array<double, nVariables>)>& fitness,
              int nIndividuos,
              int nGeneraciones,
              int umbral);

    bool evaluarPoblacion();
    void evolucionar(int nGeneraciones);
    vector<I> seleccionarPadres();
    vector<I> hacerCruzas(const vector<I>& padres, int nHijos);

    const vector<I>& individuos() const { return m_individuos; };
    double mejorFitness() const { return m_mejorAptitud; };
    const I& mejorIndividuo() const { return m_mejorIndividuo; }
    bool termino() const { return m_termino; }
    double fitnessPromdedio() const;

    static pair<I, I> cruzar(const I& padre1,
                             const I& padre2,
                             int puntoCruza,
                             const array<double, nVariables * 2>& limites);

private:
    const array<double, nVariables * 2> m_limites;
    const std::function<double(array<double, nVariables>)> m_fitness;
    const int m_nIndividuos;
    vector<I> m_individuos;
    const int m_nGeneraciones;
    const int m_umbral;
    int m_generacion;
    double m_mejorAptitud;
    I m_mejorIndividuo;
    int m_generacionesSinMejora;
    bool m_termino;
};

template <unsigned int nBits, unsigned int nVariables>
Poblacion<nBits, nVariables>::
    Poblacion(const array<double, nVariables * 2>& limites,
              const std::function<double(array<double, nVariables>)>& fitness,
              int nIndividuos,
              int nGeneraciones,
              int umbral)
    : m_limites{limites}
    , m_fitness{fitness}
    , m_nIndividuos{nIndividuos}
    , m_nGeneraciones{nGeneraciones}
    , m_umbral{umbral}
    , m_mejorAptitud{-numeric_limits<double>::max()}
    , m_mejorIndividuo{limites}
    , m_generacionesSinMejora{0}
    , m_termino{false}
{
    for (int i = 0; i < nIndividuos; ++i)
        m_individuos.push_back(I{m_limites});
}

// Devuelve true si se cumple la condición de parada por no mejorar fitness
template <unsigned int nBits, unsigned int nVariables>
bool Poblacion<nBits, nVariables>::
    evaluarPoblacion()
{
    bool mejoro = false;

    for (const I& ind : m_individuos) {
        double aptitud = m_fitness(ind.fenotipo());

        if (aptitud > m_mejorAptitud) {
            m_mejorAptitud = aptitud;
            m_mejorIndividuo = ind;
            mejoro = true;
        }
    }

    return mejoro;
}

template <unsigned int nBits, unsigned int nVariables>
void Poblacion<nBits, nVariables>::
    evolucionar(int nGeneraciones)
{
    // Si nunca se evaluó la población, hacerlo ahora
    if (m_mejorAptitud == numeric_limits<double>::min())
        evaluarPoblacion();

    int generacion;
    for (generacion = 0; generacion < nGeneraciones; ++generacion) {
        vector<I> nuevaGeneracion;
        // 1 - Rescatar el mejor individuo y meterlo en la siguiente generacion
        nuevaGeneracion.push_back(m_mejorIndividuo);

        // 2 - Seleccionar los padres
        vector<I> padres = seleccionarPadres();
        //        copy(padres.begin(), padres.end(), back_inserter(nuevaGeneracion));

        // 3 - Hacer cruzas
        //		vector<I> hijos = hacerCruzas(padres, m_nIndividuos * 0.7);
        vector<I> hijos = hacerCruzas(padres, m_nIndividuos);

        // 4 - Hacer mutaciones
        for (I& h : hijos)
            if (randu(1).at(0, 0) <= 0.1)
                h.mutar();

        //        copy(hijos.begin(), hijos.end(), back_inserter(nuevaGeneracion));
        copy(hijos.begin(), hijos.end() - 1, back_inserter(nuevaGeneracion));
        m_individuos = nuevaGeneracion;

        // 5 - Evaluar la poblacion y ver si se dio el criterio de parada
        if (evaluarPoblacion())
            m_generacionesSinMejora = 0;
        else {
            if (m_generacionesSinMejora == m_umbral) {
                m_termino = true;
                break;
            }

            ++m_generacionesSinMejora;
        }
    }
}

template <unsigned int nBits, unsigned int nVariables>
auto Poblacion<nBits, nVariables>::
    seleccionarPadres() -> vector<I>
{
    vector<I> result;
    const int nPadres = 0.3 * m_nIndividuos - 1;
    const int k = 2;

    for (int i = 0; i < nPadres; ++i) {
        const uvec indices = shuffle(linspace<uvec>(0, m_nIndividuos - 1, m_nIndividuos));

        vector<I> candidatos;
        vec aptitudes(k);
        for (int j = 0; j < k; ++j) {
            candidatos.push_back(m_individuos.at(indices(j)));
            aptitudes(j) = m_fitness(candidatos.at(j).fenotipo());
        }

        I mejor = m_individuos.at(indices(aptitudes.index_max()));
        result.push_back(mejor);
    }

    return result;
}

template <unsigned int nBits, unsigned int nVariables>
auto Poblacion<nBits, nVariables>::
    cruzar(const I& padre1,
           const I& padre2,
           int puntoCruza,
           const array<double, nVariables * 2>& limites)
        -> pair<I, I>
{
    I hijo1{limites}, hijo2{limites};

    for (int i = 0; i < int(padre1.genotipo.size()); ++i) {
        // Nota: El operador de acceso ([]) a un bitset los accede desde
        // el bit menos significativo al más significativo.

        if (i < puntoCruza) { // Parte izquierda del cromosoma
            hijo1.genotipo[i] = padre1.genotipo[i];
            hijo2.genotipo[i] = padre2.genotipo[i];
        }
        else { // Parte derecha del cromosoma
            hijo1.genotipo[i] = padre2.genotipo[i];
            hijo2.genotipo[i] = padre1.genotipo[i];
        }
    }

    return {hijo1, hijo2};
}

template <unsigned int nBits, unsigned int nVariables>
auto Poblacion<nBits, nVariables>::
    hacerCruzas(const vector<I>& padres, int nHijos) -> vector<I>
{
    vector<I> padresAux;
    vector<I> hijos;

    for (int i = 0; i < nHijos; i += 2) {
        if (padresAux.empty()) {
            padresAux = padres;
            random_shuffle(padresAux.begin(), padresAux.end());
        }
        I padre1 = padresAux.back();
        padresAux.pop_back();

        if (padresAux.empty()) {
            padresAux = padres;
            random_shuffle(padresAux.begin(), padresAux.end());
        }
        I padre2 = padresAux.back();
        padresAux.pop_back();

        int puntoCruza = randi(1, distr_param(1, nBits - 1)).at(0);

        I hijo1{m_limites}, hijo2{m_limites};

        if (randu(1).at(0, 0) <= 0.9) // 90% de probabilidad de cruza
            tie(hijo1, hijo2) = cruzar(padre1, padre2, puntoCruza, m_limites);
        else {
            hijo1 = padre1;
            hijo2 = padre2;
        }

        hijos.push_back(hijo1);
        hijos.push_back(hijo2);
    }

    return hijos;
}

template <unsigned int nBits, unsigned int nVariables>
double Poblacion<nBits, nVariables>::
    fitnessPromdedio() const
{
    double suma = 0;

    for (const I& ind : m_individuos) {
        double aptitud = m_fitness(ind.fenotipo());
        suma += aptitud;
    }

    return suma / m_individuos.size();
}
} // namespace genetico


/* ----------------------------------------------------------------------- *//**
 *
 * @file link.hpp
 *
 *//* ----------------------------------------------------------------------- */

#ifndef MADLIB_MODULES_GLM_LINK_HPP
#define MADLIB_MODULES_GLM_LINK_HPP

#include <cmath>
#include <modules/prob/boost.hpp>
#include <boost/math/distributions.hpp>
#include <boost/math/special_functions/erf.hpp>

// ------------------------------------------------------------

using namespace madlib::dbal::eigen_integration;

namespace madlib {

namespace modules {

namespace glm {

using namespace std;

// ------------------------------------------------------------

class Identity {
public:
    static double init(const double &y) { return y + 0.1; }
    static double link_func(const double &mu) { return mu; }
    static double mean_func(const double &ita) { return ita; }
    static double mean_derivative(const double &) { return 1.; }
};

// ------------------------------------------------------------

class Log {
public:
    static double init(const double &y) { return std::max(y, 0.1); }
    static double link_func(const double &mu) { return log(mu); }
    static double mean_func(const double &ita) { return exp(ita); }
    static double mean_derivative(const double &ita) { return exp(ita); }
};

// ------------------------------------------------------------

class Sqrt {
public:
    static double init(const double &y) { return std::max(y, 0.); }
    static double link_func(const double &mu) { return sqrt(mu); }
    static double mean_func(const double &ita) { return ita * ita; }
    static double mean_derivative(const double &ita) { return 2 * ita; }
};

// ------------------------------------------------------------
class Inverse {
public:
    static double init(const double &y) { return y == 0 ? 0.1 : y + 0.1; }
    static double link_func(const double &mu) { return 1./mu; }
    static double mean_func(const double &ita) { return 1./ita; }
    static double mean_derivative(const double &ita) { return -1./(ita*ita); }
};

// ------------------------------------------------------------

class SqrInverse {
public:
    static double init(const double &y) { return y == 0 ? 0.1 : y + 0.1; }
    static double link_func(const double &mu) { return 1./mu/mu; }
    static double mean_func(const double &ita) { return 1./sqrt(ita); }
    static double mean_derivative(const double &ita) { return -1./2/sqrt(ita*ita*ita); }
};

// ------------------------------------------------------------

class Probit
{
public:
    static double init(const double &y) { return (y + 0.5) / 2; }
    static double link_func(const double &mu) {
        double root_2 = sqrt(2);
        return root_2 * boost::math::erf_inv(2*mu-1);
    }

    static double mean_func(const double &ita) {
        return prob::cdf(prob::normal(), ita);
    }

    static double mean_derivative(const double &ita) {
        return exp(-ita*ita/2.)/sqrt(2*M_PI);
    }
};

// ------------------------------------------------------------

class Logit {
public:
    static double init(const double &y) { return (y + 0.5) / 2; }
    static double link_func(const double &mu) {
        return log(mu / (1 - mu));
    }
    static double mean_func(const double &ita) {
        return 1./(1 + exp(-ita));
    }
    static double mean_derivative(const double &ita) {
        return 1./((1 + exp(-ita)) * (1 + exp(ita)));
    }
};

// ------------------------------------------------------------

class MultiLogit {
public:
    static void init(ColumnVector &mu) {
        mu.fill(1./static_cast<double>(mu.size()+1)); // later we may consider to use y to initialize mu
    }
    static void link_func(const ColumnVector &mu, ColumnVector &ita) {
        for (int i=0;i<mu.size();i++)
          ita(i) = log(mu(i)) - log(1-mu.sum());
    }
    static void mean_func(const ColumnVector &ita, ColumnVector &mu) {
        double temp=0;
        for(int i=0;i<ita.size();i++)
           temp += exp(ita(i));
        temp = temp+1;
        for(int i=0;i<ita.size();i++)
           mu(i) = exp(ita(i))/temp;
    }
    static void mean_derivative(const ColumnVector &ita, Matrix &mu_prime) {
        double temp=0;
        for(int i=0;i<ita.size();i++)
           temp += exp(ita(i));
        temp = temp+1;
        for(int i=0;i<ita.size();i++) {
          for(int j=0;j<ita.size();j++) {
             if(i==j)
                mu_prime(i,j)=exp(ita(i))*(temp - exp(ita(i)))/(temp*temp);
             else
                mu_prime(i,j)=-exp(ita(i))*exp(ita(j))/(temp*temp);
          }
        }
    }
};

// ------------------------------------------------------------

class OrdinalLogit {
public:
    static void init(ColumnVector &mu) {
        mu.fill(1./static_cast<double>(mu.size()+1)); // later we may consider to use y to initialize mu
    }
    static void link_func(const ColumnVector &mu, ColumnVector &ita) {
        ColumnVector sum_vec(mu.size());
        sum_vec(0) = mu(0);
        for (int i=1;i<mu.size();i++)
            sum_vec(i) = sum_vec(i-1) + mu(i);
        for (int i=0;i<mu.size();i++)
            ita(i) = log(sum_vec(i)/(1-sum_vec(i)));
    }
    static void mean_func(const ColumnVector &ita, ColumnVector &mu) {
        mu(0) = exp(ita(0))/(1+exp(ita(0)));
        for(int i=1;i<ita.size();i++)
           mu(i) = exp(ita(i))/(1+exp(ita(i)))- exp(ita(i-1))/(1+exp(ita(i-1)));
    }
    static void mean_derivative(const ColumnVector &ita, Matrix &mu_prime) {
        mu_prime.fill(0);
        for(int i=0;i<ita.size();i++) {
            for(int j=0;j<=i;j++) {
                if(i==j)
                    mu_prime(i,j)=exp(ita(i))/(1+exp(ita(i)))/(1+exp(ita(i)));
                else if(i==(j+1))
                    mu_prime(i,j)=-exp(ita(j))/(1+exp(ita(j)))/(1+exp(ita(j)));
                else
                    mu_prime(i,j)=0;
            }
        }
    }
};

// ------------------------------------------------------------

class OrdinalProbit {
public:
    static void init(ColumnVector &mu) {
        mu.fill(1./static_cast<double>(mu.size()+1)); // later we may consider to use y to initialize mu
    }
    static void link_func(const ColumnVector &mu, ColumnVector &ita) {
        ColumnVector sum_vec(mu.size());
        double root_2 = sqrt(2);
        sum_vec(0) = mu(0);
        for (int i=1;i<mu.size();i++)
            sum_vec(i) = sum_vec(i-1) + mu(i);
        for (int i=0;i<mu.size();i++)
            ita(i) = root_2 * boost::math::erf_inv(2*sum_vec(i)-1);
    }
    static void mean_func(const ColumnVector &ita, ColumnVector &mu) {
        mu(0) = prob::cdf(prob::normal(), ita(0));
        for(int i=1;i<ita.size();i++)
            mu(i) = prob::cdf(prob::normal(), ita(i))-prob::cdf(prob::normal(), ita(i-1));
    }
    static void mean_derivative(const ColumnVector &ita, Matrix &mu_prime) {
        mu_prime.fill(0);
        for(int i=0;i<ita.size();i++) {
            for(int j=0;j<=i;j++) {
                if(i==j)
                    mu_prime(i,j) = exp(-ita(i)*ita(i)/2.)/sqrt(2*M_PI);
                else if(i==(j+1))
                    mu_prime(i,j) = -exp(-ita(j)*ita(j)/2.)/sqrt(2*M_PI);
                else
                    mu_prime(i,j)=0;
            }
        }
    }
};

} // namespace glm

} // namespace modules

} // namespace madlib

#endif // defined(MADLIB_MODULES_GLM_LINK_HPP)


#include <algorithm>
#include <cinttypes>
#include <cmath>
#include <complex>
#include <iostream>
#include <iterator>
#include <vector>

#define png_infopp_NULL nullptr
#define int_p_NULL      nullptr
#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/io/png_io.hpp>

namespace gil = boost::gil;

// map value in range r1 to r2
auto map_value(double val, double r1_from, double r1_to, double r2_from, double r2_to) -> double {
    return ((val - r1_from) / (r1_to - r1_from)) * (r2_to - r2_from) + r2_from;
}

class mandelbrot_fn {
    struct color_line {
        explicit color_line(int lv) :
            level{ lv }
        {}

        color_line(int lv, gil::bits8 r, gil::bits8 g, gil::bits8 b) :
            level{ lv },
            pixel{ r, g, b }
        {}

        int                 level;
        gil::rgb8_pixel_t   pixel;

        auto operator<(color_line const& rhs) const noexcept -> bool {
            return level < rhs.level;
        }
    };

public:
    using const_t           = mandelbrot_fn;
    using value_type        = gil::rgb8_pixel_t;
    using reference         = value_type;
    using const_reference   = value_type;
    using point_t           = gil::point2<int>;
    using result_type       = value_type;
    using argument_type     = point_t;
    static constexpr bool is_mutable = false;

    explicit mandelbrot_fn(point_t const& size) :
        m_size{ size }
    {
        m_colors.emplace_back( 0,   0,   0,  64);
        m_colors.emplace_back( 8,   0, 255, 255);
        m_colors.emplace_back(16,   0, 255,   0);
        m_colors.emplace_back(24, 255, 128,   0);
        m_colors.emplace_back(32,   0,   0, 255);
        m_colors.emplace_back(40, 255, 255,   0);
        m_colors.emplace_back(48, 255, 255, 255);
        m_colors.emplace_back(56, 255, 255, 255);
        m_colors.emplace_back(64, 255, 255, 255);
        sort(begin(m_colors), end(m_colors));
    }

    auto operator()(point_t const& point) const -> result_type {
        auto level = get_color_level(point);
        if (level < 0) {
            return result_type{ 0, 0, 0 };
        }

        auto uppos = upper_bound(begin(m_colors), end(m_colors), color_line{ level });
        if (uppos == end(m_colors)) {
            --uppos;
        }

        auto const& lower = *(uppos - 1);
        auto const& upper = *uppos;
        result_type pixel;
        for (auto i = 0; i < 3; ++i) {
            pixel[i] = map_value(level, lower.level, upper.level,
                                 lower.pixel[i], upper.pixel[i]);
        }
        return pixel;
    }

private:
    // get color level of given point
    // negative result if the point is in the set
    auto get_color_level(point_t const& point) const -> int {
        // map x to [-2, 1] and y to [1.5, -1.5]
        std::complex<double> c{map_value(point.x, 0, m_size.x, -2, 1),
                               map_value(point.y, 0, m_size.y, 1.5, -1.5)};
        int level = 0;
        for (auto lc = c; level < 64; ++level) {
            if (std::pow(lc.real(), 2) + std::pow(lc.imag(), 2) > 4) {
                return level;
            }
            lc = std::pow(lc, 2) + c;
        }
        return -1;
    }

    point_t m_size;
    std::vector<color_line> m_colors;
};

int main() {
    using point_t = mandelbrot_fn::point_t;
    using locator_t = gil::virtual_2d_locator<mandelbrot_fn, false>;
    using image_view_t = gil::image_view<locator_t>;

    point_t size{ 20'000, 20'000 };
    image_view_t view{ size, locator_t{ point_t{ 0, 0 }, point_t{ 1, 1 }, mandelbrot_fn{ size } } };
    gil::png_write_view("mandelbrot.png", view);
}


////////////////////////////////////////////////////////////////////////////////////
// The MIT License (MIT)                                                          //
//                                                                                //
// Copyright (c) 2015 Whit Armstrong                                              //
//                                                                                //
// 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.                                                                      //
////////////////////////////////////////////////////////////////////////////////////

#pragma once

#include <stdexcept>
#include <armadillo>

namespace armalogp {

  static inline double square(double x) {
    return x*x;
  }

  static inline int square(int x) {
    return x*x;
  }

  double cholesky_determinant(const arma::mat& R) {
    return arma::prod(square(R.diag()));
  }

  double mahalanobis(const arma::vec& x, const arma::vec& mu, const arma::mat& sigma) {
    const arma::vec err = x - mu;
    return arma::as_scalar(err.t() * sigma.i() * err);
  }

  double mahalanobis(const arma::rowvec& x, const arma::rowvec& mu, const arma::mat& sigma) {
    const arma::rowvec err = x - mu;
    return arma::as_scalar(err * sigma.i() * err.t());
  }

  double mahalanobis_chol(const arma::rowvec& x, const arma::rowvec& mu, const arma::mat& R) {
    const arma::rowvec err = x - mu;
    const arma::mat Rinv(inv(trimatl(R)));
    return arma::as_scalar(err * Rinv * Rinv.t() * err.t());
  }

} // namespace armalogp


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2020 Lew Wei Hao

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/exercise.hpp>
#include <boost/function.hpp>
#include <ql/math/integrals/simpsonintegral.hpp>
#include "analyticeuropeanvasicekengine.hpp"
#include <ql/math/distributions/normaldistribution.hpp>

namespace QuantLib {

    namespace {

        Real g_k(Real t, Real kappa){
            return (1 - std::exp(- kappa * t )) / kappa;
        }

        class integrand_vasicek {
          private:
            const Real sigma_s_;
            const Real sigma_r_;
            const Real correlation_;
            const Real kappa_;
            const Real T_;
          public:
            integrand_vasicek(Real sigma_s, Real sigma_r, Real correlation, Real kappa, Real T)
            : sigma_s_(sigma_s), sigma_r_(sigma_r), correlation_(correlation), kappa_(kappa), T_(T){}
            Real operator()(Real u) const {
                Real g = g_k(T_ - u, kappa_);
                return (sigma_s_ * sigma_s_) + (2 * correlation_ * sigma_s_ * sigma_r_ * g) + (sigma_r_ * sigma_r_ * g * g);
            }
        };

    }

    AnalyticBlackVasicekEngine::AnalyticBlackVasicekEngine(
            const ext::shared_ptr<GeneralizedBlackScholesProcess>& blackProcess,
            const ext::shared_ptr<Vasicek>& vasicekProcess,
            Real correlation)
    : blackProcess_(blackProcess), vasicekProcess_(vasicekProcess), simpsonIntegral_(new SimpsonIntegral(1e-5, 1000)), correlation_(correlation) {
        registerWith(blackProcess_);
        registerWith(vasicekProcess_);
    }

    void AnalyticBlackVasicekEngine::calculate() const {
        QL_REQUIRE(arguments_.exercise->type() == Exercise::European,
                   "not an European option");

        ext::shared_ptr<StrikedTypePayoff> payoff =
                ext::dynamic_pointer_cast<StrikedTypePayoff>(arguments_.payoff);

        QL_REQUIRE(payoff, "non-striked payoff given");

        CumulativeNormalDistribution f;

        Real t = 0;
        Real T = blackProcess_->riskFreeRate()->dayCounter().yearFraction(blackProcess_->riskFreeRate().currentLink()->referenceDate(),arguments_.exercise->lastDate());
        Real kappa = vasicekProcess_->a();
        Real S_t = blackProcess_->x0();
        Real K = payoff->strike();
        Real sigma_s = blackProcess_->blackVolatility()->blackVol(t, K);
        Real sigma_r = vasicekProcess_->sigma();
        Real r_t = vasicekProcess_->r0();

        Real zcb = vasicekProcess_->discountBond(t, T, r_t);
        Real epsilon = payoff->optionType() == Option::Call ? 1 : -1;
        Real upsilon = (*simpsonIntegral_)(integrand_vasicek(sigma_s, sigma_r, correlation_, kappa, T), t, T);
        Real d_positive = (std::log((S_t / K) / zcb) + upsilon / 2) / std::sqrt(upsilon);
        Real d_negative = (std::log((S_t / K) / zcb) - upsilon / 2) / std::sqrt(upsilon);
        Real n_d1 = f(epsilon * d_positive);
        Real n_d2 = f(epsilon * d_negative);

        results_.value = epsilon * ((S_t * n_d1) - (zcb * K * n_d2));
    }

}


#include "point_to_plane.h"
#include <Eigen/Dense>

using namespace probreg;

Pt2PlResult probreg::computeTwistForPointToPlane(const MatrixX3& model,
                                                 const MatrixX3& target,
                                                 const MatrixX3& target_normal,
                                                 const Vector& weight) {
    Matrix6 ata = Matrix6::Zero();
    Vector6 atb = Vector6::Zero();
    Float r_sum = 0.0;

    #pragma omp declare reduction(+ : Matrix6 : omp_out=omp_out+omp_in) initializer(omp_priv = omp_orig)
    #pragma omp declare reduction(+ : Vector6 : omp_out=omp_out+omp_in) initializer(omp_priv = omp_orig)
    #pragma omp parallel for reduction(+:ata) reduction(+:atb) reduction(+:r_sum)
    for (auto k = 0; k < model.rows(); ++k){
        const auto& vertex_k = model.row(k).transpose();
        const auto& target_k = target.row(k).transpose();
        const auto& normal_k = target_normal.row(k).transpose();
        const auto& weight_k = weight[k];
        const Float residual = normal_k.dot(target_k - vertex_k);
        const Vector6 jac = (Vector6() << vertex_k.cross(normal_k), normal_k).finished();
        const Matrix6 wjjt = weight_k * jac * jac.transpose();
        const Vector6 wrj = weight_k * residual * jac;
        const Float wr = weight_k * weight_k * residual * residual;
        ata.noalias() += wjjt;
        atb.noalias() += wrj;
        r_sum += wr;
    }
    return std::make_pair(ata.selfadjointView<Eigen::Upper>().ldlt().solve(atb), r_sum);
}


#include <fstream>
#include <algorithm>
#include <iterator>

#include <boost/functional/value_factory.hpp>
#include <boost/array.hpp>

#include <CGAL/assertions.h>
#include <CGAL/algorithm.h>
#include <CGAL/point_generators_3.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>

#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_traits.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/AABB_face_graph_triangle_primitive.h>
#include <CGAL/Timer.h>

typedef CGAL::Epeck K;
typedef K::FT FT;
typedef K::Point_3 Point;
typedef K::Vector_3 Vector;
typedef K::Segment_3 Segment;
typedef K::Ray_3 Ray;
typedef CGAL::Polyhedron_3<K> Polyhedron;
typedef CGAL::AABB_face_graph_triangle_primitive<Polyhedron> Primitive;
typedef CGAL::AABB_traits<K, Primitive> Traits;
typedef CGAL::AABB_tree<Traits> Tree;
typedef Tree::Primitive_id Primitive_id;
typedef CGAL::Timer Timer;

FT point_on_ray_dist(const Ray& ray, const Point& point) {
  Vector i_ray(point, ray.source());
  return i_ray.squared_length();
}

std::size_t accum = 0;

boost::optional<
  Tree::Intersection_and_primitive_id<Ray>::Type
  >
min_intersection(const Tree& tree, const Ray& ray) {
  typedef std::vector< Tree::Intersection_and_primitive_id<Ray>::Type > IntersectionVector;
  IntersectionVector all_intersections;

  tree.all_intersections(ray, std::back_inserter(all_intersections));
  accum += all_intersections.size();
  Tree::FT min_distance = DBL_MAX;
  boost::optional<
    Tree::Intersection_and_primitive_id<Ray>::Type
    > mini = boost::none;

  for(IntersectionVector::iterator it2 = all_intersections.begin(); it2 != all_intersections.end(); ++it2) {
    if(Point* point = boost::get<Point>(&(it2->first))) {
      Vector i_ray(*point, ray.source());
      Tree::FT new_distance = i_ray.squared_length();
      if(new_distance < min_distance) {
        mini = *it2;
        min_distance = new_distance;
      }
    } else {
      std::cout << "ERROR ignored a segment" << std::endl;
    }
  }
  return mini;
}

int main()
{
  Polyhedron polyhedron;
  {
    // Point p(1.0, 0.0, 0.0);
    // Point q(0.0, 1.0, 0.0);
    // Point r(0.0, 0.0, 1.0);
    // Point s(0.0, 0.0, 0.0);
    // polyhedron.make_tetrahedron(p, q, r, s);
  }

  std::ifstream in("data/bunny00.off");
  if(in)
    in >> polyhedron;
  else{
    std::cout << "error reading bunny" << std::endl;
    return 1;
  }

  Timer t;
  t.start();

  Tree tree(faces(polyhedron).first, faces(polyhedron).second, polyhedron);
  Tree::Bounding_box bbox = tree.bbox();
  Vector bbox_center((bbox.xmin() + bbox.xmax()) / 2,
                     (bbox.ymin() + bbox.ymax()) / 2,
                     (bbox.zmin() + bbox.zmax()) / 2);
  boost::array<double, 3> extents;
  extents[0] = bbox.xmax() - bbox.xmin();
  extents[1] = bbox.ymax() - bbox.ymin();
  extents[2] = bbox.zmax() - bbox.zmin();
  double max_extent = *std::max_element(extents.begin(), extents.end());

  std::cout << bbox << std::endl;
  std::cout << bbox_center << std::endl;
  std::cout << max_extent << std::endl;

  const int NB_RAYS = 1000;
  std::vector<Point> v1, v2;
  v1.reserve(NB_RAYS); v2.reserve(NB_RAYS);

  const double r = max_extent / 2;
  // Generate NB_RAYS*2 points that lie on a sphere of radius r, centered around bbox_center
  CGAL::Random rand = CGAL::Random(23); // fix the seed to yield the same results each run
  std::copy_n(CGAL::Random_points_on_sphere_3<Point>(r, rand), NB_RAYS, std::back_inserter(v1));
  std::copy_n(CGAL::Random_points_on_sphere_3<Point>(r, rand), NB_RAYS, std::back_inserter(v2));

  for(std::vector<Point>::iterator it = v1.begin(); it != v1.end(); ++it) {
    *it = *it + bbox_center;
  }

  for(std::vector<Point>::iterator it = v2.begin(); it != v2.end(); ++it) {
    *it = *it + bbox_center;
  }

  // Generate NB_RAYS using v1 as source and v2 as target.
  std::vector<Ray> rays;
  rays.reserve(NB_RAYS);
  std::transform(v1.begin(), v1.end(), v2.begin(),
                 std::back_inserter(rays), boost::value_factory<Ray>());
  std::vector< boost::optional<Tree::Intersection_and_primitive_id<Ray>::Type > > primitives1, primitives2;
  primitives1.reserve(NB_RAYS); primitives2.reserve(NB_RAYS);

  {
    for(std::vector<Ray>::iterator it = rays.begin(); it != rays.end(); ++it) {
      primitives1.push_back(min_intersection(tree, *it));
    }
  }

  for(std::vector<Ray>::iterator it = rays.begin(); it != rays.end(); ++it) {
    primitives2.push_back(tree.first_intersection(*it));
  }
  assert(primitives1.size() == primitives2.size()); //  Different amount of primitives intersected
  assert(std::equal(primitives1.begin(), primitives1.end(), primitives2.begin())); //  Primitives mismatch
  std::size_t c = primitives1.size() - std::count(primitives1.begin(), primitives1.end(), boost::none);
  std::cout << "Intersected " << c << " primitives with " << NB_RAYS << " rays" << std::endl;
  std::cout << "Primitive method had to sort " << accum/NB_RAYS
            << " intersections on average." << std::endl;
  t.stop();
  std::cout << t.time() << std::endl;
  return 0;
}


#include <iostream>
#include <stdexcept>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/QR>
#include <Eigen/Eigenvalues>
#include <Eigen/IterativeLinearSolvers>
#include <unsupported/Eigen/IterativeSolvers>
#include <chrono>

#include "DavidsonSolver.hpp"

DavidsonSolver::DavidsonSolver(){}

void DavidsonSolver::set_correction(std::string method) {
    if (method == "DPR") this->correction = CORR::DPR;
    else if (method == "JACOBI") this->correction = CORR::JACOBI;
    else if (method == "OLSEN") this->correction = CORR::OLSEN;
    else throw std::runtime_error("Not a valid correction method");
}

void DavidsonSolver::set_jacobi_linsolve(std::string method) {
    if (method == "CG") this->jacobi_linsolve = LSOLVE::CG;
    else if (method == "GMRES") this->jacobi_linsolve = LSOLVE::GMRES;
    else if (method == "LLT") this->jacobi_linsolve = LSOLVE::LLT;
    else throw std::runtime_error("Not a valid linsolve method");
}

Eigen::ArrayXd DavidsonSolver::_sort_index(Eigen::VectorXd& V) const
{
    Eigen::ArrayXd idx = Eigen::ArrayXd::LinSpaced(V.rows(),0,V.rows()-1);
    std::sort(idx.data(),idx.data()+idx.size(),
              [&](int i1, int i2){return V[i1]<V[i2];});
    return idx;
}

Eigen::MatrixXd DavidsonSolver::_get_initial_eigenvectors(Eigen::VectorXd &d, int size_initial_guess) const
{

    Eigen::MatrixXd guess;
    if (this->guess_vectors =="identity")
    {
            guess = Eigen::MatrixXd::Identity(d.size(),size_initial_guess);
    }

    else if (this->guess_vectors == "random")
    {
        guess = Eigen::MatrixXd::Random(d.size(),size_initial_guess);
        guess = DavidsonSolver::_QR(guess);
    }
    else if (this->guess_vectors=="target")
    {
        guess = Eigen::MatrixXd::Zero(d.size(),size_initial_guess);
        Eigen::ArrayXd idx = DavidsonSolver::_sort_index(d);

        for (int j=0; j<size_initial_guess;j++) {
            guess(idx(j),j) = 1.0;
        }
    }
    return guess;
}

Eigen::MatrixXd DavidsonSolver::_solve_linear_system(Eigen::MatrixXd &A, Eigen::VectorXd &r) const
{
    Eigen::MatrixXd w;
    std::chrono::time_point<std::chrono::system_clock> start, end;
    std::chrono::duration<double> elapsed_time;

    start = std::chrono::system_clock::now();
    switch (this->jacobi_linsolve) {

        case LSOLVE::CG :  {
                Eigen::ConjugateGradient<Eigen::MatrixXd, Eigen::Lower|Eigen::Upper> cg;
                cg.setTolerance(this->linsolve_tol);
                cg.compute(A);
                w = cg.solve(r);
            }
            break;
        case LSOLVE::GMRES : {
                Eigen::GMRES<Eigen::MatrixXd, Eigen::IdentityPreconditioner> gmres;
                gmres.setTolerance(this->linsolve_tol);
                gmres.compute(A);
                w = gmres.solve(r);
            }
            break;
        case LSOLVE::LLT :
            w = A.llt().solve(r);
            break;
    }
    end = std::chrono::system_clock::now();
    elapsed_time  = end-start;
    std::cout << "_ solve linear system " << this->jacobi_linsolve << " in " << elapsed_time.count() << " secs" <<  std::endl;
    return w;
}

Eigen::VectorXd DavidsonSolver::_olsen_correction(Eigen::VectorXd &r, Eigen::VectorXd &x, Eigen::VectorXd &D, double lambda) const
{
    /* Compute the olsen correction :

    \delta = (D-\lambda)^{-1} (-r + \epsilon x)

    */

    int size = r.rows();
    Eigen::VectorXd delta = Eigen::VectorXd::Zero(size);

    delta = DavidsonSolver::_dpr_correction(r,D,lambda);

    double _num = - x.transpose() * delta;
    double _denom = - x.transpose() * DavidsonSolver::_dpr_correction(x,D,lambda);
    double eps = _num / _denom;
    delta += eps * x;

    return delta;
}

Eigen::VectorXd DavidsonSolver::_dpr_correction(Eigen::VectorXd &w, Eigen::VectorXd &A0, double lambda) const
{
    int size = w.rows();
    Eigen::VectorXd out = Eigen::VectorXd::Zero(size);
    for (int i=0; i < size; i++) {
        out(i) = w(i) / (lambda - A0(i));
    }

    return out;
}

Eigen::MatrixXd DavidsonSolver::_QR(Eigen::MatrixXd &A) const
{

    int nrows = A.rows();
    int ncols = A.cols();
    ncols = std::min(nrows,ncols);

    Eigen::HouseholderQR<Eigen::MatrixXd> qr(A);
    return qr.householderQ() * Eigen::MatrixXd::Identity(nrows,ncols);
}

Eigen::MatrixXd DavidsonSolver::_gramschmidt( Eigen::MatrixXd &A, int nstart ) const
{
    Eigen::MatrixXd Q = A;

    for(unsigned int j = nstart; j < A.cols(); ++j) {
        // Replace inner loop over each previous vector in Q with fast matrix-vector multiplication
        Q.col(j) -= Q.leftCols(j) * (Q.leftCols(j).transpose() * A.col(j));
        // Normalize vector if possible (othw. means colums of A almsost lin. dep.
        if( Q.col(j).norm() <= 10e-14 * A.col(j).norm() ) {
            std::cerr << "Gram-Schmidt failed because A has lin. dep columns. Bye." << std::endl;
            break;
        } else {
            Q.col(j).normalize();
        }
    }
    return Q;
}


// ===========================================================================
// Imagine++ Libraries
// Copyright (C) Imagine
// For detailed information: http://imagine.enpc.fr/software
// ===========================================================================

#include "Imagine/LinAlg/MyEigen.h"
#include <Eigen/Dense>
#include <iostream>
using namespace Eigen;

template <typename T>
void subSymToMatrix (int n, const T *ap, T *t)
{
    for (int i = 0 ; i < n ; i++)
        for (int j = 0 ; j <= i ; j++)
        {
            t[i + n * j] = ap[j + (i * (i + 1) / 2)];
            t[j + n * i] = t[i + n * j];
        }
}

void symToMatrix(int n, const double *ap, double *t) { subSymToMatrix<double>(n, ap, t); }
void symToMatrix(int n, const float *ap, float *t) { subSymToMatrix<float>(n, ap, t); }

// Vectors equalization (replaces xcopy)
template <typename T>
void subEqualize(int n, const T *s, int a, T *t, int b)
{
    Map<const Matrix<T, Dynamic, 1>, 0, InnerStride<> > sMap (s, n, InnerStride<>(a));
    Map<Matrix<T, Dynamic, 1>, 0, InnerStride<> > tMap (t, n, InnerStride<>(b));
    tMap = sMap;
}

void equalize(int n, const double *s, int a, double *t, int b) { subEqualize<double>(n, s, a, t, b); }
void equalize(int n, const float *s, int a, float *t, int b) { subEqualize<float>(n, s, a, t, b); }

// Linear combination alpha * X + Y
template <typename T>
void subCombine(int n, T alpha, const T *X, T *Y)
{
    Map<const Matrix<T, Dynamic, 1>, 0, InnerStride<> > sMap (X, n, InnerStride<>(1));
    Map<Matrix<T, Dynamic, 1>, 0, InnerStride<> > tMap (Y, n, InnerStride<>(1));
    tMap = alpha * sMap + tMap;
}

void combine(int n, double alpha, const double *X, double *Y) { subCombine<double>(n, alpha, X, Y); }
void combine(int n, float alpha, const float *X, float *Y) { subCombine<float>(n, alpha, X, Y); }

// Scalar product
template <typename T>
T subScalarProduct(int n, const T *X, const T *Y)
{
    Map<const Matrix<T, Dynamic, 1>, 0, InnerStride<> > sMap (X, n, InnerStride<>(1));
    Map<const Matrix<T, Dynamic, 1>, 0, InnerStride<> > tMap (Y, n, InnerStride<>(1));
    double s = sMap.dot(tMap);
    return T(s);
}

double scalarProduct(int n, const double *X, const double *Y) { return subScalarProduct<double>(n, X, Y); }
float scalarProduct(int n, const float *X, const float *Y) { return subScalarProduct<float>(n, X, Y); }

// Product by a number
template <typename T>
void subMultiply(int n, T alpha, T *X)
{
    Map<Matrix<T, Dynamic, 1>, 0, InnerStride<> > XMap (X, n, InnerStride<>(1));
    XMap = alpha * XMap;
}

void multiply(int n, double alpha, double *X) { subMultiply<double>(n, alpha, X); }
void multiply(int n, float alpha, float *X) { subMultiply<float>(n, alpha, X); }

// Norm 2 (euclidian)
template <typename T>
T subSquaredNorm(int n, const T *X)
{
    Map<const Matrix<T, Dynamic, 1>, 0, InnerStride<> > XMap (X, n, InnerStride<>(1));
    return ((T) XMap.squaredNorm());
}

double squaredNorm(int n, const double *X) { return subSquaredNorm<double>(n, X); }
float squaredNorm(int n, const float *X) { return subSquaredNorm<float>(n, X); }

// Affine transformation Y <- alpha * [A, A transpose, A adjoint] * X + beta * B (replaces xgemv)
template <typename T>
void subAffinity(int m, int n, T alpha, const T *A, const T *X, T beta, T *Y, char t)
{
    Map<const Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > AMap (A, m, n, OuterStride<>(m));

    if (!(t == 'T' || t == 'C'))
    {
        int tmp = m;
        m = n;
        n = tmp;
    }

    Map<const Matrix<T, Dynamic, 1>, 0, InnerStride<> > XMap (X, m, InnerStride<>(1));
    Map<Matrix<T, Dynamic, 1>, 0, InnerStride<> > YMap (Y, n, InnerStride<>(1));

    if (t == 'T') YMap = alpha * AMap.transpose() * XMap + beta * YMap;
    else if (t == 'C') YMap = alpha * AMap.adjoint() * XMap + beta * YMap;
    else YMap = alpha * AMap * XMap + beta * YMap;
}

void affinity(int m, int n, double alpha, const double *A, const double *X, double beta, double *Y, char t) { subAffinity<double>(m, n, alpha, A, X, beta, Y, t); }
void affinity(int m, int n, float alpha, const float *A, const float *X, float beta, float *Y, char t) { subAffinity<float>(m, n, alpha, A, X, beta, Y, t); }

// Matrix product C <- alpha * [A, A transpose, A adjoint] * [B, B transpose, B adjoint] + beta * C (replaces xgemm)
template <typename T>
void subMatrixProduct(int m, int n, int K, T alpha, const T *A, int lda, const T *B, int ldb, T beta, T *C, char t1, char t2)
{

    Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > CMap (C, m, n, OuterStride<>(m));

    if (!(t1 == 'T' || t1 == 'C'))
    {
        int tmp = m;
        m = K;
        K = tmp;
    }

    Map<const Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > AMap (A, K, m, OuterStride<>(lda));

    if (!(t1 == 'T' || t1 == 'C'))
    {
        int tmp = m;
        m = K;
        K = tmp;
    }

    if (!(t2 == 'T' || t2 == 'C'))
    {
        int tmp = n;
        n = K;
        K = tmp;
    }

    Map<const Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > BMap (B, n, K, OuterStride<>(ldb));

    if (t1 == 'T')
        if (t2 == 'T') CMap = alpha * AMap.transpose() * BMap.transpose() + beta * CMap;
        else if (t2 == 'C') CMap = alpha * AMap.transpose() * BMap.adjoint() + beta * CMap;
        else CMap = alpha * AMap.transpose() * BMap + beta * CMap;
    else if (t1 == 'C')
        if (t2 == 'T') CMap = alpha * AMap.adjoint() * BMap.transpose() + beta * CMap;
        else if (t2 == 'C') CMap = alpha * AMap.adjoint() * BMap.adjoint() + beta * CMap;
        else CMap = alpha * AMap.adjoint() * BMap + beta * CMap;
    else
        if (t2 == 'T') CMap = alpha * AMap * BMap.transpose() + beta * CMap;
        else if (t2 == 'C') CMap = alpha * AMap * BMap.adjoint() + beta * CMap;
        else CMap = alpha * AMap * BMap + beta * CMap;
}

void matrixProduct(int m, int n, int K, double alpha, const double *A, int lda, const double *B, int ldb, double beta, double *C, char t1, char t2) { subMatrixProduct<double>(m, n, K, alpha, A, lda, B, ldb, beta, C, t1, t2); }
void matrixProduct(int m, int n, int K, float alpha, const float *A, int lda, const float *B, int ldb, float beta, float *C, char t1, char t2) { subMatrixProduct<float>(m, n, K, alpha, A, lda, B, ldb, beta, C, t1, t2); }

// Matrix/vector product (replaces xspmv)
template <typename T>
void subVectorProduct(int n,T alpha,const T *ap,const T *x,T beta,T* y)
{
    T* t=new T [n * n];
    Map<const Matrix<T, Dynamic, 1>, 0, InnerStride<> > XMap (x, n, InnerStride<>(1));
    Map<Matrix<T, Dynamic, 1>, 0, InnerStride<> > YMap (y, n, InnerStride<>(1));
    symToMatrix(n, ap, t);
    Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > apMap (t, n, n, OuterStride<>(n));
    YMap = alpha * apMap * XMap + beta * YMap;
    delete[] t;
}

void vectorProduct(int n,double alpha,const double *ap, const double *x,double beta,double* y) { subVectorProduct<double>(n, alpha, ap, x, beta, y); }
void vectorProduct(int n,float alpha,const float *ap,const float *x,float beta,float* y) { subVectorProduct<float>(n, alpha, ap, x, beta, y); }

// TODO:
//info=0 si succes, -i si le ieme parametre a une valeure illegale, i si u(i,i);=0 (transformation LU impossible = mineurs principaux tous nuls)
// LU-inverse
template <typename T>
void subMatrixInverse(int *n, T *a,int *lda,int *ipiv,int *info)
{
    Map<Matrix<T, Dynamic, Dynamic> > AMap (a,*n,*n);
    Map<VectorXi> pMap (ipiv, *n);
    Eigen::FullPivLU<Matrix<T, Dynamic, Dynamic> > lu(AMap);
    MatrixXi P = lu.permutationP();
    MatrixXi Pi = pMap;

    for (int i = 0 ; i < P.rows() - 1 ; i++)
        for (int j = i + 1 ; j < P.cols() ; j++)
            if (P(i, j) == 1) { Pi(i) = j; Pi(j) = i; };

    //pMap = Pi;

    if(lu.isInvertible() && AMap.isApprox(lu.reconstructedMatrix()))
    {
        AMap = lu.inverse();
        *info = 0;
    }
    else *info = 1;
}

void matrixInverse(int *n, double *a,int *lda,int *ipiv,int *info) { subMatrixInverse<double>(n, a, lda, ipiv, info); }
void matrixInverse(int *n, float *a,int *lda,int *ipiv,int *info) { subMatrixInverse<float>(n, a, lda, ipiv, info); }