#ifndef HAZEN_CORE
#define HAZEN_CORE
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <Eigen/QR>
#include <cmath>
#include <iostream>
#include <type_traits>
#include <utility>

namespace hazen {

// Implementation --------------------------------------------------------------
namespace Unit_impl {
using fraction = std::pair<int, int>;
constexpr int gcd(int a, int b) {
  if (a == 0)
    return b;
  return gcd(b % a, a);
}
constexpr fraction simplify(fraction frac) {
  int common_factor = gcd(frac.first, frac.second);
  fraction result = {frac.first / common_factor, frac.second / common_factor};
  if (result.second < 0) {
    result.first *= -1;
    result.second *= -1;
  }
  return result;
}
constexpr fraction add_fraction(fraction frac1, fraction frac2) {
  int den = gcd(frac1.second, frac2.second);
  den = frac1.second * frac2.second / den;
  int num =
      frac1.first * (den / frac1.second) + frac2.first * (den / frac2.second);
  return simplify({num, den});
}
constexpr fraction subtract_fraction(fraction frac1, fraction frac2) {
  frac2.first *= -1;
  return add_fraction(frac1, frac2);
}
} // namespace Unit_impl

// Unit Templates --------------------------------------------------------------
template <int L_num, int L_den, int T_num, int T_den, int M_num, int M_den,
          int Theta_num, int Theta_den>
struct Unit {
  static constexpr Unit_impl::fraction L = {L_num, L_den};
  static constexpr Unit_impl::fraction T = {T_num, T_den};
  static constexpr Unit_impl::fraction M = {M_num, M_den};
  static constexpr Unit_impl::fraction Theta = {Theta_num, Theta_den};
};

template <typename U1, typename U2> struct UPlus {
  using type = Unit<Unit_impl::add_fraction(U1::L, U2::L).first,
                    Unit_impl::add_fraction(U1::L, U2::L).second,
                    Unit_impl::add_fraction(U1::T, U2::T).first,
                    Unit_impl::add_fraction(U1::T, U2::T).second,
                    Unit_impl::add_fraction(U1::M, U2::M).first,
                    Unit_impl::add_fraction(U1::M, U2::M).second,
                    Unit_impl::add_fraction(U1::Theta, U2::Theta).first,
                    Unit_impl::add_fraction(U1::Theta, U2::Theta).second>;
};
template <typename U1, typename U2> struct UMinus {
  using type = Unit<Unit_impl::subtract_fraction(U1::L, U2::L).first,
                    Unit_impl::subtract_fraction(U1::L, U2::L).second,
                    Unit_impl::subtract_fraction(U1::T, U2::T).first,
                    Unit_impl::subtract_fraction(U1::T, U2::T).second,
                    Unit_impl::subtract_fraction(U1::M, U2::M).first,
                    Unit_impl::subtract_fraction(U1::M, U2::M).second,
                    Unit_impl::subtract_fraction(U1::Theta, U2::Theta).first,
                    Unit_impl::subtract_fraction(U1::Theta, U2::Theta).second>;
};
template <typename U, int divisor> struct UDivide {
  static constexpr Unit_impl::fraction L =
      Unit_impl::add_fraction({U::L.first, U::L.second *divisor}, {0, 1});
  static constexpr Unit_impl::fraction T =
      Unit_impl::add_fraction({U::T.first, U::T.second *divisor}, {0, 1});
  static constexpr Unit_impl::fraction M =
      Unit_impl::add_fraction({U::M.first, U::M.second *divisor}, {0, 1});
  static constexpr Unit_impl::fraction Theta = Unit_impl::add_fraction(
      {U::Theta.first, U::Theta.second *divisor}, {0, 1});
  using type = Unit<L.first, L.second, T.first, T.second, M.first, M.second,
                    Theta.first, Theta.second>;
};
template <typename U, int multiplier> struct UMultiply {
  static constexpr Unit_impl::fraction L =
      Unit_impl::add_fraction({U::L.first * multiplier, U::L.second}, {0, 1});
  static constexpr Unit_impl::fraction T =
      Unit_impl::add_fraction({U::T.first * multiplier, U::T.second}, {0, 1});
  static constexpr Unit_impl::fraction M =
      Unit_impl::add_fraction({U::M.first * multiplier, U::M.second}, {0, 1});
  static constexpr Unit_impl::fraction Theta = Unit_impl::add_fraction(
      {U::Theta.first * multiplier, U::Theta.second}, {0, 1});
  using type = Unit<L.first, L.second, T.first, T.second, M.first, M.second,
                    Theta.first, Theta.second>;
};

template <typename U1, typename U2>
using Unit_Plus = typename UPlus<U1, U2>::type;
template <typename U1, typename U2>
using Unit_Minus = typename UMinus<U1, U2>::type;
template <typename U, int divisor>
using Unit_Divide = typename UDivide<U, divisor>::type;
template <typename U, int multiplier>
using Unit_Multiply = typename UMultiply<U, multiplier>::type;

// Unit Definitions ------------------------------------------------------------
using Dimensionless_Unit = Unit<0, 1, 0, 1, 0, 1, 0, 1>;
using Length_Unit = Unit<1, 1, 0, 1, 0, 1, 0, 1>;
using Time_Unit = Unit<0, 1, 1, 1, 0, 1, 0, 1>;
using Mass_Unit = Unit<0, 1, 0, 1, 1, 1, 0, 1>;
using Temperature_Unit = Unit<0, 1, 0, 1, 0, 1, 1, 1>;
using Velocity_Unit = Unit_Minus<Length_Unit, Time_Unit>;
using Acceleration_Unit = Unit_Minus<Velocity_Unit, Time_Unit>;
using Area_Unit = Unit_Plus<Length_Unit, Length_Unit>;
using Volume_Unit = Unit_Plus<Area_Unit, Length_Unit>;
using Density_Unit = Unit_Minus<Mass_Unit, Volume_Unit>;
using Force_Unit = Unit_Plus<Mass_Unit, Acceleration_Unit>;
using Pressure_Unit = Unit_Minus<Force_Unit, Area_Unit>;
using Dynamic_Viscosity_Unit = Unit_Plus<Pressure_Unit, Time_Unit>;
using Flow_Unit = Unit_Minus<Volume_Unit, Time_Unit>;

// Scalar Templates ------------------------------------------------------------
template <typename U> class Scalar {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  Scalar<U> &operator+=(const Scalar<U> &x) {
    val += x.val;
    return *this;
  }
  Scalar<U> &operator-=(const Scalar<U> &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar<U> &s) {
    os << s.val;
    return os;
  }

  double val;
};

// Binary Arithmetic Operations
template <typename U> inline Scalar<U> operator+(Scalar<U> x, Scalar<U> y) {
  return Scalar<U>{x.val + y.val};
}
template <typename U> inline Scalar<U> operator-(Scalar<U> x, Scalar<U> y) {
  return Scalar<U>{x.val - y.val};
}
template <typename U1, typename U2>
inline Scalar<Unit_Plus<U1, U2>> operator*(Scalar<U1> x, Scalar<U2> y) {
  return Scalar<Unit_Plus<U1, U2>>{x.val * y.val};
}
template <typename U1, typename U2>
inline Scalar<Unit_Minus<U1, U2>> operator/(Scalar<U1> x, Scalar<U2> y) {
  return Scalar<Unit_Minus<U1, U2>>{x.val / y.val};
}

// Use doubles as dimensionless scalars
template <typename U> inline Scalar<U> operator*(Scalar<U> x, double y) {
  return Scalar<U>{x.val * y};
}
template <typename U> inline Scalar<U> operator*(double x, Scalar<U> y) {
  return Scalar<U>{x * y.val};
}
template <typename U> inline Scalar<U> operator/(Scalar<U> x, double y) {
  return Scalar<U>{x.val / y};
}
template <typename U>
inline Scalar<Unit_Minus<Unit<0, 1, 0, 1, 0, 1, 0, 1>, U>>
operator/(double x, Scalar<U> y) {
  return Scalar<Unit_Minus<Unit<0, 1, 0, 1, 0, 1, 0, 1>, U>>{x / y.val};
}

// Binary Comparison Operations
template <typename U> inline bool operator==(Scalar<U> x, Scalar<U> y) {
  return x.val == y.val;
}
template <typename U> inline bool operator!=(Scalar<U> x, Scalar<U> y) {
  return x.val != y.val;
}
template <typename U> inline bool operator>=(Scalar<U> x, Scalar<U> y) {
  return x.val >= y.val;
}
template <typename U> inline bool operator<=(Scalar<U> x, Scalar<U> y) {
  return x.val <= y.val;
}
template <typename U> inline bool operator>(Scalar<U> x, Scalar<U> y) {
  return x.val > y.val;
}
template <typename U> inline bool operator<(Scalar<U> x, Scalar<U> y) {
  return x.val < y.val;
}

// Scalar Functions ------------------------------------------------------------
template <int pow_num, int pow_den, typename U>
constexpr inline Scalar<Unit_Divide<Unit_Multiply<U, pow_num>, pow_den>>
power(Scalar<U> x) {
  return Scalar<Unit_Divide<Unit_Multiply<U, pow_num>, pow_den>>(std::pow(
      x.val, static_cast<double>(pow_num) / static_cast<double>(pow_den)));
}

template <typename U>
constexpr inline Scalar<Unit_Divide<U, 2>> sqrt(Scalar<U> x) {
  return Scalar<Unit_Divide<U, 2>>(std::sqrt(x.val));
}
template <typename U> constexpr inline Scalar<U> abs(Scalar<U> x) {
  return Scalar<U>(std::abs(x.val));
}
template <typename U>
constexpr inline Scalar<Unit_Multiply<U, 2>> abs2(Scalar<U> x) {
  return x * x;
}

// Scalar Result Types ---------------------------------------------------------
// Product type of two scalars
template <typename T1, typename T2>
using product_type = decltype(std::declval<T1 &>() * std::declval<T2 &>());
// Quotient type of two scalars
template <typename T1, typename T2>
using quotient_type = decltype(std::declval<T1 &>() / std::declval<T2 &>());
// Root of a scalar
template <typename T> using root_type = decltype(sqrt(std::declval<T>()));

// Scalar Definitions ----------------------------------------------------------
using Dimensionless = Scalar<Dimensionless_Unit>;
using Angle = Scalar<Dimensionless_Unit>;
using Length = Scalar<Length_Unit>;
using Time = Scalar<Time_Unit>;
using Mass = Scalar<Mass_Unit>;
using Temperature = Scalar<Temperature_Unit>;
using Velocity = Scalar<Velocity_Unit>;
using Acceleration = Scalar<Acceleration_Unit>;
using Area = Scalar<Area_Unit>;
using Volume = Scalar<Volume_Unit>;
using Density = Scalar<Density_Unit>;
using Force = Scalar<Force_Unit>;
using Pressure = Scalar<Pressure_Unit>;
using Dynamic_Viscosity = Scalar<Dynamic_Viscosity_Unit>;
using Flow = Scalar<Flow_Unit>;

template <> class Scalar<Dimensionless_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Angle Radians(double d) { return Angle(d); }
  static constexpr Angle Degrees(double d) { return Angle(d * M_PI / 180.0); }
  static Angle Slope(double rise, double run) {
    return Angle(std::atan2(rise, run));
  }
  static Angle Slope(double slope) { return Angle(std::atan(slope)); }
  static Angle Percent(double d) { return Angle(std::atan(d * 0.01)); }
  constexpr double as_radians() const { return val; }
  constexpr double as_degrees() const { return val * 180.0 / M_PI; }
  double as_slope() const { return std::tan(val); }
  double as_percent() const { return std::tan(val) * 100.0; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Length_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Length Feet(double d) { return Length(d * 0.3048); }
  static constexpr Length Yards(double d) { return Length(d * 0.3048 * 3); }
  static constexpr Length Inches(double d) { return Length(d * 0.3048 / 12); }
  static constexpr Length Meters(double d) { return Length(d); }
  constexpr double as_feet() const { return val / 0.3048; }
  constexpr double as_yards() const { return val / 0.3048 / 3; }
  constexpr double as_inches() const { return val / 0.3048 * 12; }
  constexpr double as_meters() const { return val; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Time_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Time Seconds(double d) { return Time(d); }
  constexpr double as_seconds() const { return val; }
  constexpr double as_minutes() const { return val / 60.0; }
  constexpr double as_hours() const { return val / 60.0 / 60.0; }
  constexpr double as_days() const { return val / 60.0 / 60.0 / 24.0; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Mass_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Mass Kilograms(double d) { return Mass(d); }
  static constexpr Mass Slug(double d) { return Mass(d * 14.593903); }
  constexpr double as_kilogram() const { return val; }
  constexpr double as_slug() const { return val / 14.593903; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Temperature_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Temperature Kelvin(double d) { return Temperature(d); }
  static constexpr Temperature Celcius(double d) {
    return Temperature(d + 273.15);
  }
  static constexpr Temperature Fahrenheit(double d) {
    return Temperature((d - 32) * 5 / 9 + 237.15);
  }
  constexpr double as_kelvin(double d) const { return val; }
  constexpr double as_celcius(double d) const { return val - 273.15; }
  constexpr double as_fahrenheit(double d) const {
    return (val - 273.15) * 9 / 5 + 32;
  }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Velocity_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Velocity MPS(double d) { return Velocity(d); }
  static constexpr Velocity FPS(double d) { return Velocity(d * 0.3048); }
  constexpr double as_meters_per_second() const { return val; }
  constexpr double as_feet_per_second() const { return val / 0.3048; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Acceleration_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Acceleration MPS2(double d) { return Acceleration(d); }
  static constexpr Acceleration FPS2(double d) {
    return Acceleration(d * 0.3048);
  }
  constexpr double as_meters_per_second_squared() const { return val; }
  constexpr double as_feet_per_second_squared() const { return val / 0.3048; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Area_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Area Square_Meters(double d) { return Area(d); }
  static constexpr Area Square_Feet(double d) {
    return Area(d * 0.3048 * 0.3048);
  }
  static constexpr Area Square_Inches(double d) {
    return Area(d * 0.3048 * 0.3048 / 12 / 12);
  }
  static constexpr Area Square_Yards(double d) {
    return Area(d * 0.3048 * 0.3048 * 3 * 3);
  }
  static constexpr Area Acres(double d) { return Area(d * 4046.8564224); }
  constexpr double as_square_meters() const { return val; }
  constexpr double as_square_feet() const { return val / 0.3048 / 0.3048; }
  constexpr double as_square_inches() const {
    return 12 * 12 * val / 0.3048 / 0.3048;
  }
  constexpr double as_square_yards() const {
    return val / 0.3048 / 0.3048 / 3 / 3;
  }
  constexpr double as_acres() const { return val / 4046.8564224; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Volume_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Volume Cubic_Meters(double d) { return Volume(d); }
  static constexpr Volume Cubic_Feet(double d) {
    return Volume(d * 0.3048 * 0.3048 * 0.3048);
  }
  static constexpr Volume Gallons(double d) { return Volume(d * 0.0037854118); }
  static constexpr Volume Liters(double d) { return Volume(d * 0.001); }
  constexpr double as_cubic_meters() const { return val; }
  constexpr double as_cubic_feet() const {
    return val / 0.3048 / 0.3048 / 0.3048;
  }
  constexpr double as_gallons() const { return val / 0.0037854118; }
  constexpr double as_liters() const { return val / 0.001; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Density_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Density Kilograms_per_Cubic_Meter(double d) {
    return Density(d);
  }
  static constexpr Density Slugs_per_Cubic_Foot(double d) {
    return Density(d * 515.3788184);
  }
  constexpr double as_kilograms_per_cubic_meter() { return val; }
  constexpr double as_slugs_per_cubic_foot() { return val / 515.3788184; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Force_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Force Newtons(double d) { return Force(d); }
  static constexpr Force Pounds(double d) { return Force(d * 4.4482216); }
  constexpr double as_newtons(double d) const { return val; }
  constexpr double as_pounds(double d) const { return val / 4.4482216; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Pressure_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Pressure Pascals(double d) { return Pressure(d); }
  static constexpr Pressure PSF(double d) {
    return Pressure(d * 47.880258888889);
  }
  static constexpr Pressure PSI(double d) { return Pressure(d * 6894.7572932); }
  constexpr double as_pascals() const { return val; }
  constexpr double as_psf() const { return val / 47.880258888889; }
  constexpr double as_psi() const { return val / 6894.7572932; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Dynamic_Viscosity_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Dynamic_Viscosity Pascal_Seconds(double d) {
    return Dynamic_Viscosity(d);
  }
  static constexpr Dynamic_Viscosity PSF_Seconds(double d) {
    return Dynamic_Viscosity(d * 47.880258888889);
  }
  constexpr double as_pascal_seconds() const { return val; }
  constexpr double as_psf_seconds() const { return val / 47.880258888889; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Flow_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Flow CMS(double d) { return Flow(d); }
  static constexpr Flow LPS(double d) { return Flow(d * 0.001); }
  static constexpr Flow CFS(double d) {
    return Flow(d * 0.3048 * 0.3048 * 0.3048);
  }
  static constexpr Flow GPM(double d) { return Flow(d * 0.0037854118 / 60); }
  static constexpr Flow GPD(double d) {
    return Flow(d * 0.0037854118 / 60 / 60 / 24);
  }
  static constexpr Flow MGD(double d) {
    return Flow(d * 0.0037854118 / 60 / 60 / 24 * 1000000);
  }
  constexpr double as_cubic_meters_per_second() const { return val; }
  constexpr double as_liters_per_second() const { return val / 0.001; }
  constexpr double as_cubic_feet_per_second() const {
    return val / 0.3048 / 0.3048 / 0.3048;
  }
  constexpr double as_gallons_per_minute() const {
    return val * 60 / 0.0037854118;
  }
  constexpr double as_gallons_per_day() const {
    return val * 60 * 60 * 24 / 0.0037854118;
  }
  constexpr double as_million_gallons_per_day() const {
    return val * 60 * 60 * 24 / 0.0037854118 / 1000000;
  }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};

// Unit Literals ---------------------------------------------------------------
// Meters
constexpr Length operator""_m(long double d) {
  return Length::Meters(static_cast<double>(d));
}
// Yard
constexpr Length operator""_yd(long double d) {
  return Length::Yards(static_cast<double>(d));
}
// Feet
constexpr Length operator""_ft(long double d) {
  return Length::Feet(static_cast<double>(d));
}
// Inches
constexpr Length operator""_in(long double d) {
  return Length::Inches(static_cast<double>(d));
}
// Kilograms
constexpr Mass operator""_kg(long double d) {
  return Mass::Kilograms(static_cast<double>(d));
}
// Slug
constexpr Mass operator""_slug(long double d) {
  return Mass::Slug(static_cast<double>(d));
}
// Seconds
constexpr Time operator""_s(long double d) {
  return Time::Seconds(static_cast<double>(d));
}
// Kelvin
constexpr Temperature operator""_K(long double d) {
  return Temperature::Kelvin(static_cast<double>(d));
}
// Celcius
constexpr Temperature operator""_C(long double d) {
  return Temperature::Celcius(static_cast<double>(d));
}
// Fahrenheit
constexpr Temperature operator""_F(long double d) {
  return Temperature::Fahrenheit(static_cast<double>(d));
}
// Cubic Meters
constexpr Volume operator""_m3(long double d) {
  return Volume::Cubic_Meters(static_cast<double>(d));
}
// Cubic feet
constexpr Volume operator""_ft3(long double d) {
  return Volume::Cubic_Feet(static_cast<double>(d));
}
// Liters
constexpr Volume operator""_l(long double d) {
  return Volume::Liters(static_cast<double>(d));
}
// Gallons
constexpr Volume operator""_gal(long double d) {
  return Volume::Gallons(static_cast<double>(d));
}
// Cubic Meters per Second
constexpr Flow operator""_cms(long double d) {
  return Flow::CMS(static_cast<double>(d));
}
// Cubic Feet per Second
constexpr Flow operator""_cfs(long double d) {
  return Flow::CFS(static_cast<double>(d));
}
// Gallons per Minute
constexpr Flow operator""_gpm(long double d) {
  return Flow::GPM(static_cast<double>(d));
}
// Gallons per Day
constexpr Flow operator""_gpd(long double d) {
  return Flow::GPD(static_cast<double>(d));
}
// Million Gallons per Day
constexpr Flow operator""_mgd(long double d) {
  return Flow::MGD(static_cast<double>(d));
}
// Meters per Second
constexpr Velocity operator""_mps(long double d) {
  return Velocity::MPS(static_cast<double>(d));
}
// Feet per Second
constexpr Velocity operator""_fps(long double d) {
  return Velocity::FPS(static_cast<double>(d));
}
// Meters per Second Squared
constexpr Acceleration operator""_mps2(long double d) {
  return Acceleration::MPS2(static_cast<double>(d));
}
// Feet per Second Squared
constexpr Acceleration operator""_fps2(long double d) {
  return Acceleration::FPS2(static_cast<double>(d));
}
// Kilograms per Cubic Meter
constexpr Density operator""_kgm3(long double d) {
  return Density::Kilograms_per_Cubic_Meter(static_cast<double>(d));
}
// Slug per Cubic Feet
constexpr Density operator""_slugcf(long double d) {
  return Density::Slugs_per_Cubic_Foot(static_cast<double>(d));
}
// Pascal Seconds
constexpr Dynamic_Viscosity operator""_pas(long double d) {
  return Dynamic_Viscosity::Pascal_Seconds(static_cast<double>(d));
}
// Pounds per Square Foot Second
constexpr Dynamic_Viscosity operator""_psfs(long double d) {
  return Dynamic_Viscosity::PSF_Seconds(static_cast<double>(d));
}
// Newtons
constexpr Force operator""_n(long double d) {
  return Force::Newtons(static_cast<double>(d));
}
// Pounds (Force)
constexpr Force operator""_lbf(long double d) {
  return Force::Pounds(static_cast<double>(d));
}
// Pounds (Force) per Cubic Foot
constexpr Scalar<Unit_Minus<Force_Unit, Volume_Unit>>
operator""_pcf(long double d) {
  return Scalar<Unit_Minus<Force_Unit, Volume_Unit>>{
      static_cast<double>(d * 157.0874606377)};
}
// Radians
constexpr Angle operator""_rads(long double d) {
  return Angle::Radians(static_cast<double>(d));
}
// Degrees
constexpr Angle operator""_degrees(long double d) {
  return Angle::Degrees(static_cast<double>(d));
}
// Dimensionless
constexpr Dimensionless operator""_pure(long double d) {
  return Dimensionless(static_cast<double>(d));
}

template <typename T, int rows, int cols> class Matrix {
public:
  using value_type = T;

  Matrix() = default;
  Matrix(const Matrix &) = default;
  Matrix(Matrix &&) = default;
  Matrix &operator=(const Matrix &) = default;
  Matrix &operator=(Matrix &&) = default;

  Matrix(size_t m, size_t n)
      : elems{Eigen::Matrix<double, rows, cols>::Zero(m, n)} {}
  Matrix(size_t n) : elems{Eigen::Matrix<double, rows, cols>::Zero(n)} {}
  Matrix(Eigen::Matrix<double, rows, cols> m) : elems{m} {}
  Matrix &operator=(const T &s) {
    elems.setConstant(s.val);
    return *this;
  }
  Matrix &operator=(const Eigen::Matrix<double, rows, cols> &m) {
    elems = m;
    return *this;
  }
  Matrix &operator+=(const Matrix &m) {
    elems += m.elems;
    return *this;
  }
  Matrix &operator-=(const Matrix &m) {
    elems -= m.elems;
    return *this;
  }
  Matrix &operator-() {
    elems = -elems;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Matrix &m) {
    os << m.elems;
    return os;
  }

  operator T &() { return elems(0, 0); }
  operator T() const { return elems(0, 0); }
  size_t size() const { return elems.size(); }
  size_t n_rows() const { return elems.rows(); }
  size_t n_cols() const { return elems.cols(); }

  T operator()(size_t i, size_t j = 0) const { return T(elems(i, j)); }

  static Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Constant(int m, int n,
                                                            const T &s) {
    return Matrix<T, Eigen::Dynamic, Eigen::Dynamic>{
        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Constant(m, n,
                                                                        s.val)};
  }
  static Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Random(int m, int n) {
    return Matrix<T, Eigen::Dynamic, Eigen::Dynamic>{
        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Random(m, n)};
  }
  static Matrix<T, Eigen::Dynamic, 1> Random(int n) {
    return Matrix<T, Eigen::Dynamic, 1>{
        Eigen::Matrix<double, Eigen::Dynamic, 1>::Random(n)};
  }

  Eigen::Matrix<double, rows, cols> elems;
};

// Binary Arithmetic Operations
template <typename T, int rows, int cols>
inline Matrix<T, rows, cols> operator+(const Matrix<T, rows, cols> &x,
                                       const Matrix<T, rows, cols> &y) {
  return Matrix<T, rows, cols>{x.elems + y.elems};
}
template <typename T, int rows, int cols>
inline Matrix<T, rows, cols> operator-(const Matrix<T, rows, cols> &x,
                                       const Matrix<T, rows, cols> &y) {
  return Matrix<T, rows, cols>{x.elems - y.elems};
}
template <typename T1, typename T2, int m, int n, int o>
inline Matrix<product_type<T1, T2>, m, o> operator*(const Matrix<T1, m, n> &x,
                                                    const Matrix<T2, n, o> &y) {
  return Matrix<product_type<T1, T2>, m, o>{x.elems * y.elems};
}

// Use doubles as dimensionless scalars
template <typename T, int rows, int cols>
inline Matrix<T, rows, cols> operator*(const Matrix<T, rows, cols> &x,
                                       double y) {
  return Matrix<T, rows, cols>{x.elems * y};
}
template <typename T, int rows, int cols>
inline Matrix<T, rows, cols> operator*(double x,
                                       const Matrix<T, rows, cols> &y) {
  return Matrix<T, rows, cols>{x * y.elems};
}
template <typename T, int rows, int cols>
inline Matrix<T, rows, cols> operator/(const Matrix<T, rows, cols> &x,
                                       double y) {
  return Matrix<T, rows, cols>{x.elems / y};
}
template <typename T, int rows, int cols>
inline Matrix<quotient_type<Dimensionless, T>, rows, cols>
operator/(double x, const Matrix<T, rows, cols> &y) {
  return Matrix<quotient_type<Dimensionless, T>, rows, cols>{x / y.elems};
}

// Scaling
template <typename T, typename U, int rows, int cols>
inline Matrix<product_type<T, Scalar<U>>, rows, cols>
operator*(const Matrix<T, rows, cols> &x, Scalar<U> y) {
  return Matrix<product_type<T, Scalar<U>>, rows, cols>{x.elems * y.val};
}
template <typename T, typename U, int rows, int cols>
inline Matrix<product_type<T, Scalar<U>>, rows, cols>
operator*(Scalar<U> x, const Matrix<T, rows, cols> &y) {
  return Matrix<product_type<T, Scalar<U>>, rows, cols>{x.val * y.elems};
}
template <typename T, typename U, int rows, int cols>
inline Matrix<quotient_type<T, Scalar<U>>, rows, cols>
operator/(const Matrix<T, rows, cols> &x, Scalar<U> y) {
  return Matrix<quotient_type<T, Scalar<U>>, rows, cols>{x.elems / y.val};
}
template <typename T, typename U, int rows, int cols>
inline Matrix<quotient_type<Scalar<U>, T>, rows, cols>
operator/(Scalar<U> x, const Matrix<T, rows, cols> &y) {
  return Matrix<quotient_type<Scalar<U>, T>, rows, cols>{x.val / y.elems};
}

// Binary Comparison Operations
template <typename T, int rows, int cols>
inline bool operator==(const Matrix<T, rows, cols> &x,
                       const Matrix<T, rows, cols> &y) {
  return x.elems == y.elems;
}
template <typename T, int rows, int cols>
inline bool operator!=(const Matrix<T, rows, cols> &x,
                       const Matrix<T, rows, cols> &y) {
  return x.elems != y.elems;
}
template <typename T, int rows, int cols>
inline bool operator>=(const Matrix<T, rows, cols> &x,
                       const Matrix<T, rows, cols> &y) {
  return x.elems >= y.elems;
}
template <typename T, int rows, int cols>
inline bool operator<=(const Matrix<T, rows, cols> &x,
                       const Matrix<T, rows, cols> &y) {
  return x.elems <= y.elems;
}
template <typename T, int rows, int cols>
inline bool operator>(const Matrix<T, rows, cols> &x,
                      const Matrix<T, rows, cols> &y) {
  return x.elems > y.elems;
}
template <typename T, int rows, int cols>
inline bool operator<(const Matrix<T, rows, cols> &x,
                      const Matrix<T, rows, cols> &y) {
  return x.elems < y.elems;
}

// Matrix Functions ------------------------------------------------------------
template <typename T, int rows, int cols>
T norm(const Matrix<T, rows, cols> &m) {
  return T{m.elems.norm()};
}
template <typename T, int rows, int cols>
Matrix<T, rows, cols> abs(const Matrix<T, rows, cols> &m) {
  return Matrix<T, rows, cols>{m.elems.cwiseAbs()};
}

template <typename T, int rows, int cols>
product_type<T, T> squared_norm(const Matrix<T, rows, cols> &m) {
  return product_type<T, T>{m.elems.squaredNorm()};
}
template <typename T, int rows, int cols>
Matrix<T, cols, rows> transpose(const Matrix<T, rows, cols> &m) {
  return m.elems.transpose();
}
template <typename T1, typename T2, int rows>
product_type<T1, T2> dot_product(const Matrix<T1, rows, 1> &u,
                                 const Matrix<T2, rows, 1> &v) {
  return product_type<T1, T2>(u.elems.dot(v.elems));
}
template <typename T1, typename T2, int n, int m>
Matrix<quotient_type<T2, T1>, m, 1> solve(const Matrix<T1, n, m> &A,
                                          const Matrix<T2, n, 1> &b) {
  return Matrix<quotient_type<T2, T1>, m, 1>{A.elems.lu().solve(b.elems)};
}
template <typename T1, typename T2, int n, int m>
Matrix<quotient_type<T2, T1>, m, 1>
solve_least_squares(const Matrix<T1, n, m> &A, const Matrix<T2, n, 1> &b) {
  // return Matrix<quotient_type<T2, T1>, m, 1>{
  //     A.elems.householderQr().solve(b.elems)};
  return Matrix<quotient_type<T2, T1>, m, 1>{
      A.elems.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b.elems)};
}

// Matrix and Vector aliases
template <typename T> using Mat = Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
template <typename T> using Vec = Matrix<T, Eigen::Dynamic, 1>;

} // namespace hazen

#endif


#include <float.h>
#include <cmath>
#include "get_floor_Hf.h"
#include "normalize2dpts.h"
#include <Eigen/Geometry>

using namespace Eigen;

PoseData get_floor_Hf(MatrixXd &p1, MatrixXd &p2, Matrix3d &R1, Matrix3d &R2, double f1)
{
    int nbr_coeffs = 21;
    int nbr_unknowns = 6;

    // Save copies of the inverse rotation
    Matrix3d R1T = R1.transpose();
    Matrix3d R2T = R2.transpose();

    // Compute normalization matrix
    double scale = normalize2dpts(p2);
    Vector3d s;
    s << scale, scale, 1.0;
    DiagonalMatrix<double, 3> S = s.asDiagonal();

    // Initialize known calibration matrix
    Vector3d k1inv;
    k1inv << 1 / f1, 1 / f1, 1.0;
    DiagonalMatrix<double, 3> K1inv = k1inv.asDiagonal();

    // Normalize data
    Matrix3d y1;
    Matrix3d x2;
    y1 = R1T * K1inv * p1.colwise().homogeneous();
    x2 = p2.colwise().homogeneous();

    x2 = S * x2;

    MatrixXd y1t(2,3);
    y1t << y1.colwise().hnormalized();
    MatrixXd x2t(2,3);
    x2t << x2.colwise().hnormalized();

    // Wrap input data to expected format
    VectorXd input(nbr_coeffs);
    input << y1t.col(0),
             x2t.col(0),
             y1t.col(1),
             x2t.col(1),
             y1t.col(2),
             x2t.col(2),
             Map<VectorXd>(R2T.data(), 9);

    // TODO: Not sure if this is necessary (assure const)
    const Map<VectorXd> input_data(input.data(), nbr_coeffs);

    // Extract solution
    MatrixXcd sols = solver_floor_Hf(input_data);

    // Pre-processing: Remove complex-valued solutions
    double thresh = 1e-5;
    ArrayXd real_sols(7);
    real_sols = sols.imag().cwiseAbs().colwise().sum();
    int nbr_real_sols = (real_sols <= thresh).count();

    // Allocate space for putative (real) homographies
    MatrixXd best_homography(3, 3);
    double best_focal_length;
    double best_algebraic_error = DBL_MAX;
    double algebraic_error;

    // Since this is a 2.5 pt solver, use the last
    // (previously unused) constraint, to discard
    // false solutions.
    ArrayXd xx(nbr_unknowns);
    VectorXd input_algebraic(nbr_coeffs + nbr_unknowns);

    for (int i = 0; i < real_sols.size(); i++) {
        if (real_sols(i) <= thresh) {
            // Compute algebraic error, and compare to other solutions.
            xx = sols.col(i).real();
            input_algebraic << xx, input;
            algebraic_error = get_algebraic_error_floor_Hf(input_algebraic);

            if (algebraic_error < best_algebraic_error) {
                best_algebraic_error = algebraic_error;
                best_homography << xx[0], xx[2], xx[1],
                                       0, xx[3],     0,
                                  -xx[1], xx[4], xx[0];
                best_focal_length = xx[5];
            }
        }
    }

    // Construct homography
    Matrix3d K, H;
    K = Vector3d(best_focal_length, best_focal_length, 1).asDiagonal();
    // Ki = Vector3d(1, 1, best_focal_length).asDiagonal();
    H = S.inverse() * K * R2 * best_homography * R1.transpose() * K1inv;

    // Package output
    PoseData posedata;
    posedata.homography = H;
    posedata.focal_length = best_focal_length / scale;

    return posedata;
}

// ---------------- //
// MATLAB interface //
// ---------------- //
#ifdef MATLAB_MEX_FILE
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	if (nrhs != 5) {
		mexErrMsgIdAndTxt("get_floor_Hf:nrhs", "Five input arguments are required.");
	}
	if (nlhs != 2) {
		mexErrMsgIdAndTxt("get_floor_Hf:nlhs", "Two output arguments are required.");
	}
	if (!mxIsDouble(prhs[0]) || mxIsComplex(prhs[0])) {
		mexErrMsgIdAndTxt("get_floor_Hf:notDouble", "Input data must be type double.");
	}
	if(mxGetNumberOfElements(prhs[0]) != 6 && mxGetNumberOfElements(prhs[1]) != 6 && mxGetNumberOfElements(prhs[2]) != 9 && mxGetNumberOfElements(prhs[3]) != 9 && mxGetNumberOfElements(prhs[4]) != 1) {
		mexErrMsgIdAndTxt("get_floor_Hf:incorrectSize", "Input dimensions incorrect.");
	}
    // Convert to expected input
    VectorXd x1_tmp = Map<VectorXd>(mxGetPr(prhs[0]), 6);
    VectorXd x2_tmp = Map<VectorXd>(mxGetPr(prhs[1]), 6);
    MatrixXd x1 = Map<MatrixXd>(x1_tmp.data(), 2, 3);
    MatrixXd x2 = Map<MatrixXd>(x2_tmp.data(), 2, 3);

    VectorXd R1_tmp = Map<VectorXd>(mxGetPr(prhs[2]), 9);
    VectorXd R2_tmp = Map<VectorXd>(mxGetPr(prhs[3]), 9);
    Matrix3d R1 = Map<Matrix3d>(R1_tmp.data(), 3, 3);
    Matrix3d R2 = Map<Matrix3d>(R2_tmp.data(), 3, 3);

    double *f1 = mxGetPr(prhs[4]);

    // Compute output
	PoseData posedata = get_floor_Hf(x1, x2, R1, R2, f1[0]);

    // Wrap it up to Matlab compatible output
	plhs[0] = mxCreateDoubleMatrix(3, 3, mxREAL);
	double* zr = mxGetPr(plhs[0]);
    for (Index i = 0; i < posedata.homography.size(); i++) {
        zr[i] = posedata.homography(i);
    }

    plhs[1] = mxCreateDoubleMatrix(1, 1, mxREAL);
    zr = mxGetPr(plhs[1]);
    zr[0] = posedata.focal_length;
}
#endif

// Function that utilizes the last equation of the DLT system to discard false solutions
double get_algebraic_error_floor_Hf(VectorXd &data)
{
    const double* d = data.data();

    // Compute algebraic error
    double error;
    error = -d[0]*d[5]*d[14]*d[25] - d[0]*d[14]*d[16]*d[19] - d[0]*d[14]*d[17]*d[22] - d[1]*d[5]*d[25] - d[1]*d[16]*d[19] - d[1]*d[17]*d[22] - d[2]*d[5]*d[15]*d[25] - d[2]*d[15]*d[16]*d[19] - d[2]*d[15]*d[17]*d[22] + d[3]*d[5]*d[15]*d[24] + d[3]*d[15]*d[16]*d[18] + d[3]*d[15]*d[17]*d[21];

    return abs(error);
}


#include <sequential-line-search/preferenceregressor.h>
#include <sequential-line-search/utils.h>
#include <iostream>
#include <fstream>
#include <cmath>
#include <Eigen/LU>
#include <nlopt-util.hpp>

using Eigen::MatrixXd;
using Eigen::VectorXd;

//#define VERBOSE
//#define NOISELESS

#ifdef VERBOSE
#include <timer.hpp>
#endif

namespace
{
    using namespace sequential_line_search;

#ifdef NOISELESS
    const double b_fixed = 1e-06;
#endif

    inline double calc_grad_a(const VectorXd& y, const MatrixXd& C_inv, const MatrixXd& X, double a, double b, const VectorXd& r)
    {
        const double   a_prior  = PreferenceRegressor::Params::getInstance().a;
        const double   variance = PreferenceRegressor::Params::getInstance().variance;

        const MatrixXd C_grad_a             = Regressor::calc_C_grad_a(X, a, b, r);
        const double   log_p_f_theta_grad_a = 0.5 * y.transpose() * C_inv * C_grad_a * C_inv * y - 0.5 * (C_inv * C_grad_a).trace();
        const double   log_prior            = (std::log(a_prior) - variance - std::log(a)) / (variance * a);
        return log_p_f_theta_grad_a + log_prior;
    }

#ifndef NOISELESS
    inline double calc_grad_b(const VectorXd& y, const MatrixXd& C_inv, const MatrixXd& X, double a, double b, const VectorXd& r)
    {
        const double   b_prior  = PreferenceRegressor::Params::getInstance().b;
        const double   variance = PreferenceRegressor::Params::getInstance().variance;

        const MatrixXd C_grad_b             = Regressor::calc_C_grad_b(X, a, b, r);
        const double   log_p_f_theta_grad_b = 0.5 * y.transpose() * C_inv * C_grad_b * C_inv * y - 0.5 * (C_inv * C_grad_b).trace();
        const double   log_prior            = (std::log(b_prior) - variance - std::log(b)) / (variance * b);
        return log_p_f_theta_grad_b + log_prior;
    }
#endif

    inline VectorXd calc_grad_r(const VectorXd& y, const MatrixXd& C_inv, const MatrixXd& X, double a, double b, const VectorXd& r)
    {
        const double   r_prior  = PreferenceRegressor::Params::getInstance().r;
        const double   variance = PreferenceRegressor::Params::getInstance().variance;

        VectorXd grad = VectorXd::Zero(r.rows());
        for (unsigned i = 0; i < r.rows(); ++ i)
        {
            const MatrixXd C_grad_r               = Regressor::calc_C_grad_r_i(X, a, b, r, i);
            const double   log_p_f_theta_grad_r_i = 0.5 * y.transpose() * C_inv * C_grad_r * C_inv * y - 0.5 * (C_inv * C_grad_r).trace();
            grad(i) += log_p_f_theta_grad_r_i;
        }
        for (unsigned i = 0; i < r.rows(); ++ i)
        {
            const double log_prior = (std::log(r_prior) - variance - std::log(r(i))) / (variance * r(i));
            grad(i) += log_prior;
        }

        return grad;
    }

    // log p(d_k | f)
    inline double calc_log_likelihood(const Preference& p, const double w, const VectorXd& y)
    {
        const double btl_scale = w * PreferenceRegressor::Params::getInstance().btl_scale;

        VectorXd tmp(p.size()); for (unsigned i = 0; i < p.size(); ++ i) tmp(i) = y(p[i]);
        return std::log(utils::BTL(tmp, btl_scale));
    }

    // Log likelihood that will be maximized
    double objective(const std::vector<double> &x, std::vector<double>& grad, void* data)
    {
        const PreferenceRegressor* regressor = static_cast<PreferenceRegressor*>(data);

        const MatrixXd&                X = regressor->X;
        const std::vector<Preference>& D = regressor->D;
        const VectorXd&                w = regressor->w;
        const unsigned                 M = X.cols();
        const VectorXd                 y = Eigen::Map<const VectorXd>(&x[0], M);

        const double   a = (regressor->use_MAP_hyperparameters) ? x[M + 0] : PreferenceRegressor::Params::getInstance().a;
#ifdef NOISELESS
        const double   b = b_fixed;
#else
        const double   b = (regressor->use_MAP_hyperparameters) ? x[M + 1] : PreferenceRegressor::Params::getInstance().b;
#endif
        const VectorXd r = (regressor->use_MAP_hyperparameters) ? VectorXd(Eigen::Map<const VectorXd>(&x[M + 2], X.rows())) : VectorXd::Constant(X.rows(), PreferenceRegressor::Params::getInstance().r);

        double obj = 0.0;

        // Log likelihood of data
        for (unsigned i = 0; i < D.size(); ++ i)
        {
            obj += calc_log_likelihood(D[i], w(i), y);
        }

        // Log likelihood of y distribution
        const MatrixXd C     = Regressor::calc_C(X, a, b, r);
        const MatrixXd C_inv = C.inverse();
        const double   C_det = C.determinant();
        const double   term1 = - 0.5 * y.transpose() * C_inv * y;
        const double   term2 = - 0.5 * std::log(C_det);
        const double   term3 = - 0.5 * M * std::log(2.0 * M_PI);
        obj += term1 + term2 + term3;

        if (regressor->use_MAP_hyperparameters)
        {
            // Priors for GP parameters
            const double   a_prior  = PreferenceRegressor::Params::getInstance().a;
#ifndef NOISELESS
            const double   b_prior  = PreferenceRegressor::Params::getInstance().b;
#endif
            const double   r_prior  = PreferenceRegressor::Params::getInstance().r;
            const double   variance = PreferenceRegressor::Params::getInstance().variance;

            obj += std::log(utils::log_normal(a, std::log(a_prior), variance));
#ifndef NOISELESS
            obj += std::log(utils::log_normal(b, std::log(b_prior), variance));
#endif
            for (unsigned i = 0; i < r.rows(); ++ i)
            {
                obj += std::log(utils::log_normal(r(i), std::log(r_prior), variance));
            }
        }

        // When the algorithm is gradient-based, compute the gradient vector
        if (grad.size() == x.size())
        {
            VectorXd grad_y = VectorXd::Zero(y.rows());

            // Accumulate per-data derivatives
            const double btl_scale = PreferenceRegressor::Params::getInstance().btl_scale;
            for (unsigned i = 0; i < D.size(); ++ i)
            {
                const Preference& p = D[i];
                const double      s = btl_scale * w(i);
                VectorXd tmp1(p.size()); for (unsigned i = 0; i < p.size(); ++ i) tmp1(i) = y(p[i]);
                const VectorXd tmp2 = utils::derivative_BTL(tmp1, s) / utils::BTL(tmp1, s);
                for (unsigned i = 0; i < p.size(); ++ i) grad_y(p[i]) += tmp2(i);
            }

            // Add GP term
            grad_y += - C_inv * y;

            Eigen::Map<VectorXd>(&grad[0], grad_y.rows()) = grad_y;

            if (regressor->use_MAP_hyperparameters)
            {
                grad[M + 0] = calc_grad_a(y, C_inv, X, a, b, r);
#ifdef NOISELESS
                grad[M + 1] = 0.0;
#else
                grad[M + 1] = calc_grad_b(y, C_inv, X, a, b, r);
#endif
                VectorXd grad_r = calc_grad_r(y, C_inv, X, a, b, r);
                for (unsigned i = 0; i < grad_r.rows(); ++ i)
                {
                    grad[M + 2 + i] = grad_r(i);
                }
            }
            else
            {
                grad[M + 0] = 0.0;
                grad[M + 1] = 0.0;
                grad[M + 2] = 0.0;
            }
        }

        return obj;
    }

}

namespace sequential_line_search
{
    PreferenceRegressor::PreferenceRegressor(const MatrixXd &X, const std::vector<Preference>& D, bool use_MAP_hyperparameters) :
    use_MAP_hyperparameters(use_MAP_hyperparameters),
    X(X),
    D(D)
    {
        if (X.cols() == 0 || D.size() == 0) return;

        w = Eigen::VectorXd::Ones(D.size());

        compute_MAP();

        C     = calc_C(X, a, b, r);
        C_inv = C.inverse();
    }

    PreferenceRegressor::PreferenceRegressor(const MatrixXd &X, const std::vector<Preference>& D, const Eigen::VectorXd &w, bool use_MAP_hyperparameters) :
    use_MAP_hyperparameters(use_MAP_hyperparameters),
    X(X),
    D(D),
    w(w)
    {
        if (X.cols() == 0 || D.size() == 0) return;

        compute_MAP();

        C     = calc_C(X, a, b, r);
        C_inv = C.inverse();
    }

    PreferenceRegressor::PreferenceRegressor(const MatrixXd &X, const std::vector<Preference>& D, const Eigen::VectorXd &w, bool use_MAP_hyperparameters, const PreferenceRegressor* previous) :
    use_MAP_hyperparameters(use_MAP_hyperparameters),
    X(X),
    D(D),
    w(w)
    {
        if (X.cols() == 0 || D.size() == 0) return;

        compute_MAP(previous);

        C     = calc_C(X, a, b, r);
        C_inv = C.inverse();
    }

    double PreferenceRegressor::estimate_y(const VectorXd &x) const
    {
        const VectorXd k = Regressor::calc_k(x, X, a, b, r);
        return k.transpose() * C_inv * y;
    }

    double PreferenceRegressor::estimate_s(const VectorXd &x) const
    {
        const VectorXd k = Regressor::calc_k(x, X, a, b, r);
        return std::sqrt(a + b - k.transpose() * C_inv * k);
    }

    void PreferenceRegressor::compute_MAP(const PreferenceRegressor *previous)
    {
        const unsigned M = X.cols();
        const unsigned d = X.rows();

        VectorXd upper = VectorXd::Constant(M + 2 + d, + 1e+01);
        VectorXd lower = VectorXd::Constant(M + 2 + d, - 1e+01); lower.block(M, 0, 2 + d, 1) = VectorXd::Constant(2 + d, 1e-05);
        VectorXd x_ini = VectorXd::Constant(M + 2 + d, 0.0);
        x_ini(M + 0) = Params::getInstance().a;
        x_ini(M + 1) = Params::getInstance().b;
        x_ini.block(M + 2, 0, d, 1) = VectorXd::Constant(d, Params::getInstance().r);

        // Use the MAP estimated values in previous regression as initial values
        if (previous != nullptr)
        {
            for (unsigned i = 0; i < M; ++ i)
            {
                x_ini(i) = previous->estimate_y(X.col(i));
            }
            x_ini(M + 0) = previous->a;
            x_ini(M + 1) = previous->b;
            x_ini.block(M + 2, 0, d, 1) = previous->r;
        }

#ifdef VERBOSE
        timer::Timer t("PreferenceRegressor::compute_MAP");
#endif

        const VectorXd x_opt = nloptutil::solve(x_ini, upper, lower, objective, nlopt::LD_TNEWTON, this, 500);

        y = x_opt.block(0, 0, M, 1);

        if (use_MAP_hyperparameters)
        {
            a = x_opt(M + 0);
#ifdef NOISELESS
            b = b_fixed;
#else
            b = x_opt(M + 1);
#endif
            r = x_opt.block(M + 2, 0, d, 1);

#ifdef VERBOSE
            std::cout << "Learned hyperparameters ... a: " << a << ", \tb: " << b << ", \tr: " << r.transpose() << std::endl;
#endif
        }
        else
        {
            a = PreferenceRegressor::Params::getInstance().a;
            b = PreferenceRegressor::Params::getInstance().b;
            r = VectorXd::Constant(d, PreferenceRegressor::Params::getInstance().r);
        }
    }

    ///////////////////////////////////////////////////////////////////

#if 0
    namespace
    {

        double objective_function(const std::vector<double> &x, std::vector<double>& grad, void* data)
        {
            const PreferenceRegressor* regressor = static_cast<PreferenceRegressor*>(data);

            const unsigned M = x.size();

            if (!grad.empty())
            {
                const VectorXd  x_vec = Eigen::Map<const VectorXd>(&x[0], M);
                const MatrixXd& X     = regressor->X;
                const MatrixXd& C_inv = regressor->C_inv;
                const VectorXd& y     = regressor->y;
                const unsigned  N     = y.rows();
                const double    a     = regressor->a;
                const double    l     = regressor->l;

                MatrixXd k_derivative(M, N);

                for (unsigned i = 0; i < N; ++ i)
                {
                    const double   tmp            = (- 0.5 / (l * l)) * (x_vec - X.col(i)).squaredNorm();
                    const VectorXd k_i_derivative = a * std::exp(tmp) * (- 1.0 / (l * l) * (x_vec - X.col(i)));

                    k_derivative.col(i) = k_i_derivative;
                }

                const MatrixXd grad_vec = k_derivative * (C_inv * y);
                Eigen::Map<VectorXd>(&grad[0], M) = grad_vec;
            }

            return regressor->estimate_y(Eigen::Map<const VectorXd>(&x[0], M));
        }

    }
#endif

    VectorXd PreferenceRegressor::find_arg_max()
    {
        const unsigned M = X.rows();

        assert (M != 0);

        int i; y.maxCoeff(&i);
#if 0
        const VectorXd x_initial = X.col(i);
        const VectorXd upper     = VectorXd::Constant(M, 1.0);
        const VectorXd lower     = VectorXd::Constant(M, 0.0);

        return nloptutils::compute(x_initial, upper, lower, objective_function, static_cast<void*>(this), nlopt::LD_TNEWTON, 100);
#else
        return X.col(i);
#endif
    }

    void PreferenceRegressor::dampData(const std::string &dirPath) const
    {
        // Export X using CSV
        utils::exportMatrixToCsv(dirPath + "/X.csv", X);

        // Export D using CSV
        std::ofstream ofs_D(dirPath + "/D.csv");
        for (unsigned i = 0 ; i < D.size(); ++ i)
        {
            for (unsigned j = 0; j < D[i].size(); ++ j)
            {
                ofs_D << D[i][j];
                if (j + 1 != D[i].size()) ofs_D << ",";
            }
            ofs_D << std::endl;
        }
    }
}


/*
   Copyright (C) 2016-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

     http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/

// Calculating free energy density of quantum antiferomagnetic Heisenberg chain

#pragma once

#include <Eigen/Dense>
#include "standards/exp_number.hpp"
#include "chain.hpp"

namespace afh { namespace free_energy { namespace chain {

class finite {
  typedef std::size_t uint_t;
  typedef Eigen::VectorXd vector_t;
  typedef Eigen::MatrixXd matrix_t;
public:
  finite(uint_t L) : L_(L), eigenvalues_(1 << L_) {
    // lattice
    std::vector<std::pair<uint_t, uint_t> > lattice;
    for (uint_t i = 0; i < L_; ++i) lattice.push_back(std::make_pair(i, (i+1) % L_));

    // generate Hamiltonian
    uint_t dim = 1 << L_;
    matrix_t hamiltonian(dim, dim);
    afh::free_energy::chain::generate(L_, lattice, hamiltonian);

    /* perform eigenvalue decomposition */
    Eigen::SelfAdjointEigenSolver<matrix_t> eigensolver(hamiltonian);
    eigenvalues_ = eigensolver.eigenvalues();
  }
  double gs_energy() const { return eigenvalues_(0) / L_; }
  double gap() const { return eigenvalues_(1) - eigenvalues_(0); }
  std::tuple<double, double, double> free_energy(double t) const {
    // calculate free energy and intternal energy
    uint_t dim = 1 << L_;
    double beta = 1 / t;
    standards::exp_double z = 0;
    standards::exp_double w = 0;
    for (uint_t i = 0; i < dim; ++i) {
      uint_t j = dim - i - 1;
      z += standards::exp_double::exp(-beta * eigenvalues_(j));
      w += eigenvalues_(j) * standards::exp_double::exp(-beta * eigenvalues_(j));
    }
    double f = - log(z) / (beta * L_);
    double e = w / z / L_;
    return std::make_tuple(f, e, beta * (e-f));
  }
private:
  uint_t L_;
  vector_t eigenvalues_;
};

} } }


/**
 * \file dcs/math/random/mersenne_twister.hpp
 *
 * \brief Mersenne Twister Random Number Engine.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_RANDOM_MERSENNE_TWISTER_HPP
#define DCS_MATH_RANDOM_MERSENNE_TWISTER_HPP

#include <dcs/detail/config/boost.hpp>

#if !DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(101500) // 1.15
#	error "Required Boost libraries version >= 1.15."
#endif

#include <boost/random/mersenne_twister.hpp>
#include <dcs/math/random/base_generator.hpp>
#include <cstddef>
#include <stdint.h>

namespace dcs { namespace math { namespace random {

/**
 * \brief A mersenne_twister random number engine produces unsigned integer
 * random numbers in the closed interval \f$[0, 2^w − 1]\f$.
 *
 * \tparam UIntT The type of randomly generated numbers.
 * \tparam w The word size.
 * \tparam n The state size.
 * \tparam m The shift size.
 * \tparam r The mask bits.
 * \tparam a The XOR mask.
 * \tparam u The tempering.
 * \tparam s The tempering.
 * \tparam b The tempering.
 * \tparam t The tempering.
 * \tparam c The parameter.
 * \tparam l The tempering.
 *
 * A mersenne_twister random number engine produces unsigned integer random
 * numbers in the closed interval \f$[0, 2^{nw-r} − 1]\f$.
 * The state \f$x_i\f$ of a mersenne_twister object \f$x\f$ is of size \f$n\f$
 * and consists of a sequence \f$X\f$ of \f$n\f$ values of the type delivered by
 * \f$x\f$; all subscripts applied * to \f$X\f$ are to be taken modulo \f$n\f$.
 * The transition algorithm employs a twisted generalized feedback shift
 * register defined by shift values \f$n\f$ and \f$m\f$, a twist value \f$r\f$,
 * and a conditional xor-mask \f$a\f$.
 * To improve the uniformity of the result, the bits of the raw shift register
 * are additionally tempered (i.e., scrambled) according to a bit-scrambling
 * matrix defined by values \f$u\f$, \f$s\f$, \f$b\f$, \f$t\f$,
 * \f$c\f$, and \f$l\f$.
 * The state transition is performed as follows:
 * a) Concatenate the upper \f$w-r\f$ bits of \f$X_{i-n}\f$ with the lower
 *    \f$r\f$ bits of \f$X_{i+1-n}\f$ to obtain an unsigned integer value \f$Y\f$.
 * b) With \f$\alpha=a*(Y \wedge 1)\f$, set \f$X_i\f$ to
 *    \f$X_{i+m-n} \oplus (Y >> 1) xor \alpha\f$
 * The generation algorithm determines the unsigned integer values \f$z_1\f$,
 * \f$z_2\f$, \f$z_3\f$, \f$z_4\f$ as follows, then delivers \f$z_4\f$ as its
 * result:
 * a) Let \f$z_1 = X_i \oplus ((X_i >> u)\f$.
 * b) Let \f$z_2 = z_1 \oplus ((z_1 << s) \wedge b)\f$.
 * c) Let \f$z_3 = z_2 \oplus ((z_2 << t) \wedge c)\f$.
 * d) Let \f$z_4 = z_3 \oplus (z_3 >> l).
 *
 * This class implements the \c RandomNumberEngine concept.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <
	typename UIntT,
	::std::size_t w,
	::std::size_t n,
	::std::size_t m,
	::std::size_t r,
	UIntT a,
	::std::size_t u,
	::std::size_t s,
	UIntT b,
	::std::size_t t,
	UIntT c,
	::std::size_t l
>
class mersenne_twister: public base_generator<UIntT>
{
	private: typedef base_generator<UIntT> base_type;
	public: typedef UIntT result_type;
	public: typedef typename base_type::ulonglong_type ulonglong_type;
	private: typedef ::boost::random::mersenne_twister<UIntT,w,n,m,r,a,u,s,b,t,c,l,0> impl_type;

	public: static const ::std::size_t word_size = w;
	public: static const ::std::size_t state_size = n;
	public: static const ::std::size_t shift_size = m;
	public: static const ::std::size_t mask_bits = r;
	public: static const result_type xor_mask = a;
	public: static const ::std::size_t tempering_u = u;
	public: static const ::std::size_t tempering_s = s;
	public: static const result_type tempering_b = b;
	public: static const ::std::size_t tempering_t = t;
	public: static const result_type parameter_c = c;
	public: static const ::std::size_t tempering_l = l;
	public: static const result_type default_seed = impl_type::default_seed;

	public: mersenne_twister()
		: impl_()
	{
		// empty
	}

	public: explicit mersenne_twister(result_type z)
		: impl_(z)
	{
		// empty
	}

//	public: template <typename ItT>
//		mersenne_twister(ItT& first, ItT& last)
//		: impl_(first,last)
//	{
//		// empty
//	}

//	public: template <typename ItT>
//		void seed(ItT& first, ItT& last)
//	{
//		impl_.seed(first, last);
//	}

	public: static result_type min()
	{
		//return impl_type::min();//FIXME: has not been implemented yet
		return 0;
	}

	public: static result_type max()
	{
		//return impl_type::max();//FIXME: has not been implemented yet
		// 2^w-1
		result_type res = 0;
		for(size_t i = 0; i < w; ++i)
		{
			res |= (1u << i);
		}
		return res;
	}

	private: result_type do_min() const
	{
		return impl_.min();
	}

	private: result_type do_max() const
	{
		return impl_.max();
	}

	private: void do_seed()
	{
		impl_.seed();
	}

	private: void do_seed(result_type z)
	{
		impl_.seed(z);
	}

	private: result_type do_generate()
	{
		return impl_();
	}

	//FIXME: actually cannot use impl_.discard since it is defined only when
	//       BOOST_NO_LONG_LONG is undefined.
	private: void do_discard(ulonglong_type z)
	{
		for ( ; z != 0; --z)
		{
			this->operator()();
		}

//		impl_.discard(z);
	}

	public: friend bool operator==(mersenne_twister const& x, mersenne_twister const& y)
	{
		return x.impl_ == y.impl_;
	}

	public: friend bool operator!=(mersenne_twister const& x, mersenne_twister const& y)
	{
		return !(x == y);
	}

	private: impl_type impl_;
};

typedef mersenne_twister<uint32_t, 32, 351, 175, 19, 0xccab8ee7, 11, 7, 0x31b6ab00, 15, 0xffe50000, 17> mt11213b;
typedef mersenne_twister<uint32_t, 32, 624, 397, 31, 0x9908b0df, 11, 7, 0x9d2c5680, 15, 0xefc60000, 18> mt19937;

}}} // Namespace dcs::math::random

#endif // DCS_MATH_RANDOM_MERSENNE_TWISTER_HPP


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#pragma once
#include <eve/detail/skeleton_calls.hpp>
#include <eve/detail/hz_device.hpp>
#include <eve/concept/value.hpp>
#include <eve/function/if_else.hpp>
#include <eve/function/oneminus.hpp>
#include <eve/function/normal_distribution.hpp>
#include <eve/function/lognormal_distribution.hpp>
#include <eve/function/diff/gamma_p.hpp>
#include <eve/function/abs.hpp>
#include <eve/function/any.hpp>
#include <eve/function/average.hpp>
#include <eve/function/none.hpp>
#include <eve/function/converter.hpp>
#include <eve/function/dec.hpp>
#include <eve/function/is_eqz.hpp>
#include <eve/function/is_not_nan.hpp>
#include <eve/function/is_lez.hpp>
#include <eve/function/is_not_less.hpp>
#include <eve/function/lgamma.hpp>
#include <eve/function/log.hpp>
#include <eve/function/log1p.hpp>
#include <eve/function/max.hpp>
#include <eve/function/min.hpp>
#include <eve/function/rec.hpp>
#include <eve/function/sqr.hpp>
#include <eve/function/sqrt.hpp>
#include <eve/constant/one.hpp>
#include <eve/constant/inf.hpp>
#include <eve/constant/eps.hpp>
#include <eve/function/ulpdist.hpp>
#include <eve/function/next.hpp>
#include <eve/function/nb_values.hpp>
#include <eve/constant/valmax.hpp>
#include <eve/constant/inf.hpp>
#include <type_traits>
#include <boost/math/special_functions/gamma.hpp>
#include <tts/tts.hpp>
#include <iomanip>
namespace eve::detail
{

  template<real_value T, real_value U>
  EVE_FORCEINLINE  auto gamma_pinv_(EVE_SUPPORTS(cpu_)
                              , T a
                              , U b) noexcept
  requires compatible_values<T, U>
  {
    return arithmetic_call(gamma_pinv, a, b);
  }

  template<floating_real_value T>
  EVE_FORCEINLINE T gamma_pinv_(EVE_SUPPORTS(cpu_), T p, T k) noexcept
  requires has_native_abi_v<T>
  {
    if constexpr(std::is_same_v<T, float>)
    {
      return float32(gamma_pinv(float64(p), float64(k)));
    }
    p = if_else(is_ltz(p) || p > one(as(p)), allbits, p);
    auto iseqzp = is_eqz(p);
    auto iseq1p = p == one(as(p));
    auto x = if_else(iseq1p, inf(as(p)), if_else(iseqzp, zero(as(p)), allbits));
    logical<T> notdone(is_not_nan(p) && !iseqzp && !iseq1p);
    auto d = rec(9*k);
    auto y = oneminus(d + invcdf(normal_distribution_01<T>, oneminus(p)) * eve::sqrt(d));

    x = if_else(notdone, k*sqr(y)*y, x);
    auto x0 = x;
    int i = 10;
    if (eve::none(notdone)) return x;
    while(i)
    {
      auto dx = if_else(notdone, (gamma_p(x, k)-p)/diff(gamma_p)(x, k), zero);
      x -= dx;
      if (i < 7) notdone = notdone && is_not_less(abs(dx), 4*eps(as(x))*max(eve::abs(x), one(as(x))));
      if (eve::none(notdone)) return x;
      --i;
    }
    notdone =  notdone || is_ltz(y);
    x = if_else(notdone, eve::abs(x0), x);
    auto xlo = if_else(notdone, eve::min(x/2, zero(as(x))), x);
    auto xhi = if_else(notdone, eve::min(x*2, eve::valmax(as(x))), x);
    auto inl = ((gamma_p(xlo, k) > p)||(gamma_p(xhi, k) <  p)) && (xlo !=  xhi);
    while (eve::any(inl))
    {
      xlo = if_else(inl, eve::max(xlo/2, zero(as(x))), xlo);
      xhi  = if_else(inl, eve::min(xhi*2, eve::valmax(as(x))), xhi);
      inl = ((gamma_p(xlo, k) > p)||(gamma_p(xhi, k) <  p)) && (xlo !=  xhi);
    }
    auto xmed = average(xlo, xhi);
    while (eve::any(notdone))
    {
      auto test = (gamma_p(xmed, k) <  p);
      xlo = if_else(test, xmed, xlo);
      xhi = if_else(test, xhi, xmed);
      notdone = ulpdist(xlo, xhi) > 1;
      xmed = average(xlo, xhi);
    }
    xmed = if_else(iseq1p, inf(as(p)), if_else(iseqzp, zero(as(p)), xmed));
    return if_else(k == one(as(k)),  -eve::log1p(-p), xmed);
  }
}


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

/*
 Copyright (C) 2015 Klaus Spanderen

 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.
*/

/*! \file analytichestonadengine.hpp
  \brief analytic pricing engine for a heston option
  based on fourier transformation
*/

#include <ql/math/functional.hpp>
#include <ql/instruments/payoffs.hpp>
#include <ql/math/integrals/gaussianadquadratures.hpp>
#include <ql/pricingengines/vanilla/analytichestonadengine.hpp>

#include <boost/assign/std/vector.hpp>
using namespace boost::assign;

namespace QuantLib {

	namespace {
		std::complex<CppAD::AD<Real> > operator*(
			Real a, const std::complex<CppAD::AD<Real> >& b ) {
			return std::complex<CppAD::AD<Real> >(a*b.real(), a*b.imag());
		}
		std::complex<CppAD::AD<Real> > operator*(
			const std::complex<CppAD::AD<Real> >& b, Real a) {
			return a*b;
		}
		std::complex<CppAD::AD<Real> > operator-(
			Real a, const std::complex<CppAD::AD<Real> >& b) {
			return std::complex<CppAD::AD<Real> >(a - b.real(), -b.imag());
		}
	}

    // helper class for integration
    class AnalyticHestonADEngine::Fj_Helper
        : public std::unary_function<Real, Real>
    {
    public:
        Fj_Helper(
        	CppAD::AD<Real>& kappa,
        	CppAD::AD<Real>& theta,
        	CppAD::AD<Real>& sigma,
        	CppAD::AD<Real>& v0,
        	CppAD::AD<Real>& s0,
        	CppAD::AD<Real>& rho,
            const AnalyticHestonADEngine* const engine,
            ComplexLogFormula cpxLog,
            Time term,
            Real strike,
            Real ratio,
            Size j);

        CppAD::AD<Real> operator()(Real phi)      const;

    private:
        const Size j_;
        const CppAD::AD<Real> &kappa_, &theta_, &sigma_, &v0_;
        const ComplexLogFormula cpxLog_;

        // helper variables
        const Time term_;
        CppAD::AD<Real> x_;
        const Real sx_;
        CppAD::AD<Real> dd_;
        const CppAD::AD<Real> sigma2_, rsigma_;
        const CppAD::AD<Real> t0_;

        // log branch counter
        mutable int  b_;     // log branch counter
        mutable Real g_km1_; // imag part of last log value

        const AnalyticHestonADEngine* const engine_;
    };

    AnalyticHestonADEngine::Fj_Helper::Fj_Helper(
		CppAD::AD<Real>& kappa,
		CppAD::AD<Real>& theta,
		CppAD::AD<Real>& sigma,
		CppAD::AD<Real>& v0,
		CppAD::AD<Real>& s0,
		CppAD::AD<Real>& rho,
        const AnalyticHestonADEngine* const engine,
        ComplexLogFormula cpxLog,
        Time term,
        Real strike,
        Real ratio,
        Size j)
        :
        j_(j),
        kappa_(kappa),
        theta_(theta),
        sigma_(sigma),
        v0_(v0),
        cpxLog_(cpxLog),
        term_(term),
        x_(log(s0)),
        sx_(std::log(strike)),
        dd_(x_-std::log(ratio)),
        sigma2_(sigma_*sigma_),
        rsigma_(rho*sigma_),
        t0_(kappa - ((j== 1)? rho*sigma : 0)),
        b_(0),
        g_km1_(0),
        engine_(engine) {
    }

    CppAD::AD<Real> AnalyticHestonADEngine::Fj_Helper::operator()(
    	Real phi) const {

        const CppAD::AD<Real> rpsig(rsigma_*phi);

        const std::complex<CppAD::AD<Real> > t1 =
        	t0_+std::complex<CppAD::AD<Real> >(0, -rpsig);
        const std::complex<CppAD::AD<Real> > d =
            std::sqrt(t1*t1 - sigma2_*phi
                      *std::complex<CppAD::AD<Real> >(-phi, (j_== 1)? 1 : -1));
        const std::complex<CppAD::AD<Real> > ex = exp(-d*term_);

        if (cpxLog_ == Gatheral) {
            if (phi != 0.0) {
                if (sigma_ > 1e-5) {
                    const std::complex<CppAD::AD<Real> > p = (t1-d)/(t1+d);
                    const std::complex<CppAD::AD<Real> > g
                        = log((1.0 - p*ex)/(1.0 - p));

                    const std::complex<CppAD::AD<Real> > c(
                    	v0_*(t1-d)*(1.0-ex)/(sigma2_*(1.0-ex*p))
                    	+ (kappa_*theta_)/sigma2_*((t1-d)*term_-2.0*g));

                    CppAD::AD<Real> im(c.imag()), re(c.real());

                    return
                        exp(std::complex<CppAD::AD<Real> >(re, im)
							 + std::complex<CppAD::AD<Real> >(0.0, phi*(dd_-sx_))
							 ).imag()/phi;
                }
                else {
                	QL_FAIL("ouch, sigma < 1e-5");
                }
            }
            else {
            	QL_FAIL("ouch, phi = 0");
            }
        }
        else if (cpxLog_ == BranchCorrection) {
        	QL_FAIL("ouch, BranchCorrection");
        }
        else {
            QL_FAIL("unknown complex logarithm formula");
        }
    }

    AnalyticHestonADEngine::AnalyticHestonADEngine(
                              const boost::shared_ptr<HestonModel>& model,
                              Size integrationOrder)
    : GenericModelEngine<HestonModel,
                         VanillaOption::arguments,
                         VanillaOption::results>(model),
      evaluations_(0),
      cpxLog_     (Gatheral),
      integration_(new Integration(
                          Integration::gaussLaguerre(integrationOrder))) {
    }

    Size AnalyticHestonADEngine::numberOfEvaluations() const {
        return evaluations_;
    }

    void AnalyticHestonADEngine::doCalculation(Real riskFreeDiscount,
                                             Real dividendDiscount,
                                             Real spotPrice,
                                             Real strikePrice,
                                             Real term,
                                             Real kappa, Real theta, Real sigma, Real v0, Real rho,
                                             const TypePayoff& type,
                                             const Integration& integration,
                                             const ComplexLogFormula cpxLog,
                                             const AnalyticHestonADEngine* const enginePtr,
                                             VanillaOption::results& results,
                                             Size& evaluations)
    {
        std::vector<CppAD::AD<Real> > params;
        params += spotPrice, v0, kappa, theta, sigma, rho;
        CppAD::Independent(params);

        std::vector<Real> vp;
        vp += spotPrice, v0, kappa, theta, sigma, rho;
        const Real ratio = riskFreeDiscount/dividendDiscount;

        evaluations = 0;

        const CppAD::AD<Real> p1 = integration.calculate(
            Fj_Helper(params[2], params[3], params[4],
            		  params[1], params[0], params[5], enginePtr,
                      cpxLog, term, strikePrice, ratio, 1))/M_PI;
        evaluations+= integration.numberOfEvaluations();

        const CppAD::AD<Real> p2 = integration.calculate(
            Fj_Helper(params[2], params[3], params[4],
            		  params[1], params[0], params[5], enginePtr,
                      cpxLog, term, strikePrice, ratio, 2))/M_PI;
        evaluations+= integration.numberOfEvaluations();

        std::vector<CppAD::AD<Real> > y(1);

        switch (type.optionType())
        {
          case Option::Call:
            y[0] = params[0]*dividendDiscount*(p1+0.5)
            	- strikePrice*riskFreeDiscount*(p2+0.5);
            break;
          case Option::Put:
        	  y[0] = params[0]*dividendDiscount*(p1-0.5)
                - strikePrice*riskFreeDiscount*(p2-0.5);
            break;
          default:
            QL_FAIL("unknown option type");
        }

        std::vector<Real> moreResults
        	= CppAD::ADFun<Real>(params, y).Reverse(1, std::vector<Real>(1, 1.0));

        results.value = CppAD::Value(y[0]);

        results.delta = moreResults[0];
        results.additionalResults["delta"] = moreResults[0];
        results.additionalResults["v0"]    = moreResults[1];
        results.additionalResults["kappa"] = moreResults[2];
        results.additionalResults["theta"] = moreResults[3];
        results.additionalResults["sigma"] = moreResults[4];
        results.additionalResults["rho"]   = moreResults[5];
    }

    void AnalyticHestonADEngine::calculate() const
    {
        // this is a european option pricer
        QL_REQUIRE(arguments_.exercise->type() == Exercise::European,
                   "not an European option");

        // plain vanilla
        boost::shared_ptr<PlainVanillaPayoff> payoff =
            boost::dynamic_pointer_cast<PlainVanillaPayoff>(arguments_.payoff);
        QL_REQUIRE(payoff, "non plain vanilla payoff given");

        const boost::shared_ptr<HestonProcess>& process = model_->process();

        const Real riskFreeDiscount = process->riskFreeRate()->discount(
                                            arguments_.exercise->lastDate());
        const Real dividendDiscount = process->dividendYield()->discount(
                                            arguments_.exercise->lastDate());

        const Real spotPrice = process->s0()->value();
        QL_REQUIRE(spotPrice > 0.0, "negative or null underlying given");

        const Real strikePrice = payoff->strike();
        const Real term = process->time(arguments_.exercise->lastDate());

        doCalculation(riskFreeDiscount,
                      dividendDiscount,
                      spotPrice,
                      strikePrice,
                      term,
                      model_->kappa(),
                      model_->theta(),
                      model_->sigma(),
                      model_->v0(),
                      model_->rho(),
                      *payoff,
                      *integration_,
                      cpxLog_,
                      this,
                      results_,
                      evaluations_);
    }

    AnalyticHestonADEngine::Integration::Integration(
            Algorithm intAlgo,
            const boost::shared_ptr<GaussianADQuadrature>& gaussianQuadrature)
    : intAlgo_(intAlgo),
      gaussianQuadrature_(gaussianQuadrature) { }

    AnalyticHestonADEngine::Integration
    AnalyticHestonADEngine::Integration::gaussLaguerre(Size intOrder) {
        QL_REQUIRE(intOrder <= 192, "maximum integraton order (192) exceeded");
        return Integration(GaussLaguerre,
                           boost::shared_ptr<GaussianADQuadrature>(
                               new GaussLaguerreADIntegration(intOrder)));
    }

    AnalyticHestonADEngine::Integration
    AnalyticHestonADEngine::Integration::gaussLegendre(Size intOrder) {
        return Integration(GaussLegendre,
                           boost::shared_ptr<GaussianADQuadrature>(
                               new GaussLegendreADIntegration(intOrder)));
    }

    AnalyticHestonADEngine::Integration
    AnalyticHestonADEngine::Integration::gaussChebyshev(Size intOrder) {
        return Integration(GaussChebyshev,
                           boost::shared_ptr<GaussianADQuadrature>(
                               new GaussChebyshevADIntegration(intOrder)));
    }

    AnalyticHestonADEngine::Integration
    AnalyticHestonADEngine::Integration::gaussChebyshev2nd(Size intOrder) {
        return Integration(GaussChebyshev2nd,
                           boost::shared_ptr<GaussianADQuadrature>(
                               new GaussChebyshev2ndADIntegration(intOrder)));
    }

    Size AnalyticHestonADEngine::Integration::numberOfEvaluations() const {
    	return gaussianQuadrature_->order();
    }

    CppAD::AD<Real> AnalyticHestonADEngine::Integration::calculate(
	    const boost::function<CppAD::AD<Real>(Real)>& f) const {

    	CppAD::AD<Real> retVal;

        switch(intAlgo_) {
          case GaussLaguerre:
            retVal = gaussianQuadrature_->operator()(f);
            break;
          default:
              QL_FAIL("unknwon integration algorithm");
        }

        return retVal;
     }
}


#include "unscented.h"

#include <iostream>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <unsupported/Eigen/MatrixFunctions>

using namespace uvdar;

double angdiff(double x,double y){
  double d = y-x;
  if ((d>-M_PI)&&(d<M_PI)) return d;
  return fmod(d+M_PI,2*M_PI)-M_PI;
}

unscented::measurement unscented::unscentedTransform(e::VectorXd x,e::MatrixXd Px, const boost::function<e::VectorXd(e::VectorXd,e::VectorXd,int)> &fcn,double fleft,double fright, double fcenter, int camera_index){
  int L = x.rows();
  double W0=1.0/3.0;
  Eigen::MatrixXd X(L,2*L+1);
  X.leftCols(1) = x;
  X.rightCols(2*L).Ones(L,2*L);

  Eigen::VectorXd W(2*L+1);
  W =e::VectorXd::Ones(2*L+1)*((1-W0)/(2*L));
  W(0)=W0;

  e::MatrixXd sf = ((L/(1-W0))*Px).sqrt();

  for (int i=0; i<L; i++){
    X.col(i*2+1) = (x+(sf.row(i)).transpose());
    X.col(i*2+2) = (x-(sf.row(i)).transpose());// check
  }
  e::MatrixXd Y(6,2*L+1);

  e::VectorXd expFrequencies;
  if (fcenter>0){
    expFrequencies = e::VectorXd(3);
    expFrequencies << fleft,fright,fcenter;
  }
  else {
    expFrequencies = e::VectorXd(2);
    expFrequencies << fleft,fright;
  }

  for (int i=0; i<(1+2*L); i++){
    Y.col(i)=fcn(X.col(i),expFrequencies, camera_index); //this is weird, check please
  }
  int nan_index = -1;
  int nan_count = 0;
  for (int i=0; i<(1+2*L); i++){
    if (Y.col(i).array().isNaN().any()){
      nan_count++;
      nan_index = i;
    }
  }

  if (nan_count == 1){
    Y.col(nan_index) = Y.col(0);
  }
  if (false){
  /* if (true){ */
    std::cout << "unscented W: "<< std::endl;
    std::cout << W.transpose() << std::endl;
    std::cout << "unscented X: "<< std::endl;
    std::cout << X << std::endl;
    std::cout << "unscented Y: "<< std::endl;
    std::cout << Y << std::endl;
  }
  e::Vector3d mr;
    mr << 0,0,0;
  e::VectorXd y = Y*W;

  e::MatrixXd Ye = (Y-y.replicate(1,2*L+1));
  e::MatrixXd Py = ((Ye*W.asDiagonal())*Ye.transpose());

  for (int i=0; i<3; i++){
      if (y(3+i)>M_PI){
        y(3+i)=-2*M_PI+y(3+i);
    }
  }
  struct measurement output;
  output.x = y;
  output.C = Py;
  return output;
}

std::vector<Eigen::VectorXd> unscented::getSigmaPtsSource(e::VectorXd x,e::MatrixXd Px){
  int L = x.rows();
  double W0=1.0/3.0;
  std::vector<e::VectorXd> output;
  output.push_back(x);
  Eigen::VectorXd W(2*L+1);
  W =e::VectorXd::Ones(2*L+1)*((1-W0)/(2*L));
  W(0)=W0;
  e::MatrixXd sf = ((L/(1-W0))*Px).sqrt();
  for (int i=0; i<L; i++){
    output.push_back((x+(sf.row(i)).transpose()));
    output.push_back((x-(sf.row(i)).transpose()));
  }
  return output;
}

/* } */


/* compile_time_factorial.hpp
 *
 * Calculating factorial at compile time using recursive templates instantiation
 * Requires Boost.Multiprecision library
*/

#ifndef _COMPILE_TIME_FACTORIAL_HPP_
#define _COMPILE_TIME_FACTORIAL_HPP_

#include <type_traits>

#include <boost/multiprecision/cpp_int.hpp>

#ifdef __RECURSIVE_TEMPLATE_INSTANTIATION_FACTORIAL_COMPUTING
#warning "Be careful when using large numbers!"
#warning "Add the -ftemplate-depth=N flag to increase the depth of template instantiation!"
#endif // __RECURSIVE_TEMPLATE_INSTANTIATION_FACTORIAL_COMPUTING

namespace alex::utils::math
{
    namespace concepts
    {
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        template<typename T>
        concept IntegerType =
#if defined(__RECURSIVE_TEMPLATE_INSTANTIATION_FACTORIAL_COMPUTING) || defined(__RECURSIVE_CONSTEXPR_FUNCTION_FACTORIAL_COMPUTING) ||   \
    defined(__CONSTEXPR_FUNCTION_FOR_LOOP_FACTORIAL_COMPUTING)
            std::is_integral_v<T> || boost::multiprecision::is_number<T>::value;
#else
            std::is_integral_v<T>;
#endif
#else
    template<typename T>
    struct IsIntegerType
    {
        static constexpr bool value {
#if defined(__RECURSIVE_TEMPLATE_INSTANTIATION_FACTORIAL_COMPUTING) || defined(__RECURSIVE_CONSTEXPR_FUNCTION_FACTORIAL_COMPUTING) ||   \
    defined(__CONSTEXPR_FUNCTION_FOR_LOOP_FACTORIAL_COMPUTING)
            std::is_integral_v<T> || boost::multiprecision::is_number<T>::value
#else
            std::is_integral_v<T>
#endif
        };
    };
#endif
    }; // namespace alex::utils::math::concepts

#if defined(__RECURSIVE_CONSTEXPR_FUNCTION_FACTORIAL_COMPUTING) || defined(__CONSTEXPR_FUNCTION_FOR_LOOP_FACTORIAL_COMPUTING)
    namespace detail
    {
        template<size_t N>
        constexpr size_t cached_factorial_value {
            [] {
                if constexpr (N <= 1)       return 1;
                else if constexpr (N == 2)  return 2;
                else if constexpr (N == 3)  return 6;
                else if constexpr (N == 4)  return 24;
                else if constexpr (N == 5)  return 120;
            }()
        };
    } // namespace alex::utils::math::ct_factorial::detail
#endif

#ifdef __RECURSIVE_TEMPLATE_INSTANTIATION_FACTORIAL_COMPUTING

    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::IntegerType integer_type,
        size_t N
#else
        typename integer_type,
        size_t N,
        typename = std::enable_if_t<concepts::IsIntegerType<integer_type>::value>
#endif
    >
    struct ct_factorial { static constexpr integer_type value{N * ct_factorial<integer_type, N - 1>::value}; };

    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::IntegerType integer_type
#else
        typename integer_type
#endif
    >
    struct ct_factorial<integer_type, 0> { static constexpr integer_type value{1}; };

#elif defined(__RECURSIVE_CONSTEXPR_FUNCTION_FACTORIAL_COMPUTING)

    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::IntegerType integer_type,
        size_t N
#else
        typename integer_type,
        size_t N,
        typename = std::enable_if_t<concepts::IsIntegerType<integer_type>::value>
#endif
    >
    constexpr integer_type ct_factorial()
    {
        if constexpr (N <= 5) return detail::cached_factorial_value<N>;
        else return N * ct_factorial<integer_type, N - 1>();
    }

#elif defined(__CONSTEXPR_FUNCTION_FOR_LOOP_FACTORIAL_COMPUTING)

    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::IntegerType integer_type,
        size_t N
#else
        typename integer_type,
        size_t N,
        typename = std::enable_if_t<concepts::IsIntegerType<integer_type>::value>
#endif
    >
    constexpr integer_type ct_factorial()
    {
        if constexpr (N <= 5) return detail::cached_factorial_value<N>;
        else {
            integer_type result{1};
            for (size_t i{1}; i <= N; ++i) {
                result *= i;
            }
            return result;
        }
    }

#elif defined(__SFINAE_TEMLATE_INSTANTIATION_FACTORIAL_COMPUTING)

    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::IntegerType integer_type,
        size_t N
#else
        typename integer_type,
        size_t N,
        typename = std::enable_if_t<concepts::IsIntegerType<integer_type>::value>
#endif
    >
    struct ct_factorial : std::integral_constant<integer_type, N * ct_factorial<integer_type, N - 1>{}> {};

    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::IntegerType integer_type
#else
        typename integer_type
#endif
    >
    struct ct_factorial<integer_type, 0> : std::integral_constant<integer_type, 1> {};

#endif

} // namespace alex::utils::math

#endif // _COMPILE_TIME_FACTORIAL_HPP_


/**
 * @file
 * @brief NPDE homework "Handling degrees of freedom (DOFs) in LehrFEM++"
 * @author Julien Gacon
 * @date March 1st, 2019
 * @copyright Developed at ETH Zurich
 */

#include "lfppdofhandling.h"

#include <Eigen/Dense>
#include <array>
#include <memory>

#include "lf/assemble/assemble.h"
#include "lf/base/base.h"
#include "lf/geometry/geometry.h"
#include "lf/mesh/mesh.h"
#include "lf/mesh/utils/utils.h"

namespace LFPPDofHandling {

/* SAM_LISTING_BEGIN_1 */
std::array<std::size_t, 3> countEntityDofs(
    const lf::assemble::DofHandler &dofhandler) {
  std::array<std::size_t, 3> entityDofs;
  // Idea: iterate over entities in the mesh and get interior number of dofs for
  // each
  std::shared_ptr<const lf::mesh::Mesh> mesh = dofhandler.Mesh();
  for (std::size_t codim = 0; codim <= 2; ++codim) {
    entityDofs[codim] = 0;
    for (const auto *el : mesh->Entities(codim)) {
      if (el->RefEl() == lf::base::RefEl::kQuad()) {
        throw "Only triangular meshes are allowed!";
      }
      entityDofs[codim] += dofhandler.NumInteriorDofs(*el);
    }
  }
  return entityDofs;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
std::size_t countBoundaryDofs(const lf::assemble::DofHandler &dofhandler) {
  std::shared_ptr<const lf::mesh::Mesh> mesh = dofhandler.Mesh();
  // given an entity, bd\_flags(entity) == true, if the entity is on the
  // boundary
  lf::mesh::utils::AllCodimMeshDataSet<bool> bd_flags(
      lf::mesh::utils::flagEntitiesOnBoundary(mesh));
  std::size_t no_dofs_on_bd = 0;
  // Edges and nodes can be on the boundary
  for (const auto *edge : mesh->Entities(1)) {
    if (bd_flags(*edge)) {
      no_dofs_on_bd += dofhandler.NumInteriorDofs(*edge);
    }
  }
  for (const auto *node : mesh->Entities(2)) {
    if (bd_flags(*node)) {
      no_dofs_on_bd += dofhandler.NumInteriorDofs(*node);
    }
  }
  return no_dofs_on_bd;
}
/* SAM_LISTING_END_2 */

// clang-format off
/* SAM_LISTING_BEGIN_3 */
double integrateLinearFEFunction(
    const lf::assemble::DofHandler& dofhandler,
    const Eigen::VectorXd& mu) {
  double I = 0;
  std::shared_ptr<const lf::mesh::Mesh> mesh = dofhandler.Mesh();
  std::cout << "indices\n";
  for (const auto *cell : mesh->Entities(0)) {
    // check if we the FE space is really $\Cs_1^0$
    if (dofhandler.NumLocalDofs(*cell) != 3) {
      throw "Not a S_1^0 FE space!";
    }
    // iterate over dofs
    auto int_dofs = dofhandler.GlobalDofIndices(*cell);

    for (auto dof_idx_p = int_dofs.begin(); dof_idx_p < int_dofs.end();
         ++dof_idx_p) {
      // local integral of the basis function associated with this dof:
      // in linear Lagrangian FE, the integral over the basis functions over
      // a triangle K is: 1/3*vol(K)
      const double I_bary = 1.0 / 3.0 * lf::geometry::Volume(*(cell->Geometry()));
      // multiply by the value at the dof to get local contribution
    std::cout << *dof_idx_p << "\n";
      I += I_bary * mu(*dof_idx_p);
    }
  }
  return I;
}
/* SAM_LISTING_END_3 */
// clang-format on

/* SAM_LISTING_BEGIN_4 */
double integrateQuadraticFEFunction(const lf::assemble::DofHandler &dofhandler,
                                    const Eigen::VectorXd &mu) {
  double I = 0;
  std::shared_ptr<const lf::mesh::Mesh> mesh = dofhandler.Mesh();
  for (const auto *cell : mesh->Entities(0)) {
    // check if we the FE space is really $\Cs_2^0$
    if (dofhandler.NumLocalDofs(*cell) != 6) {
      throw "Not a S_2^0 FE space!";
    }
    const double weight = 1.0 / 3.0 * lf::geometry::Volume(*(cell->Geometry()));
    // iterate over dofs
    auto int_dofs = dofhandler.GlobalDofIndices(*cell);
    // The integrated basis functions associated with the nodes are 0:
    //  $\int_K b_K^j dx = 0$ for $j = 1,2,3$. Skip!
    for (int l = 3; l < 6; ++l) {
      // The integrated basis functions associated with the edges are:
      // $\int_K b_K^j dx = |K|/3$ for $j = 4,5,6$
      // multiply by the value at the dof to get local contribution
      I += (weight * mu(int_dofs[l]));
    }
  }
  return I;
}
/* SAM_LISTING_END_4 */

/* SAM_LISTING_BEGIN_5 */
Eigen::VectorXd convertDOFsLinearQuadratic(
    const lf::assemble::DofHandler &dofh_Linear_FE,
    const lf::assemble::DofHandler &dofh_Quadratic_FE,
    const Eigen::VectorXd &mu) {
  if (dofh_Linear_FE.Mesh() != dofh_Quadratic_FE.Mesh()) {
    throw "Underlying meshes must be the same for both DOF handlers!";
  }
  std::shared_ptr<const lf::mesh::Mesh> mesh =
      dofh_Linear_FE.Mesh();                          // get the mesh
  Eigen::VectorXd zeta(dofh_Quadratic_FE.NumDofs());  // initialise empty zeta
  // safety guard: always set zero if you're not sure to set every entry later
  // on for us this shouldn't be a problem, but just to be sure
  zeta.setZero();

  for (const auto *cell : mesh->Entities(0)) {
    // check if the spaces are actually linear and quadratic
    if (dofh_Linear_FE.NumLocalDofs(*cell) != 3 ||
        dofh_Quadratic_FE.NumLocalDofs(*cell) != 6) {
      throw "dofh_Linear_FE must have 3 dofs per cell and dofh_Quadratic_FE 6!";
    }
    // get the global dof indices of the linear and quadratic FE spaces, note
    // that the vectors obey the LehrFEM++ numbering, which we will make use of
    // lin\_dofs will have size 3 for the 3 dofs on the nodes and
    // quad\_dofs will have size 6, the first 3 entries being the nodes and
    // the last 3 the edges
    nonstd::span<const lf::assemble::gdof_idx_t> lin_dofs =
        dofh_Linear_FE.GlobalDofIndices(*cell);
    nonstd::span<const lf::assemble::gdof_idx_t> quad_dofs =
        dofh_Quadratic_FE.GlobalDofIndices(*cell);
    for (std::size_t l = 0; l <= 2; ++l) {
      // Let $p_1,p_2,p_3$ be the nodes of the triangle and $p_4,p_5,p_6$ the
      // edge midpoints. Let $\lambda_1,\lambda_2,\lambda_3$ be the local basis
      // functions of $\Cs_1^0$ and $b_1,..,b_6$ for $\Cs_2^0$. The values of
      // $\lambda_i$ in the interpolation nodes $p_1,..,p_6$ are the
      // coefficients of writing $\lambda_i$ as lin. comb of $b_1,..b_6$.
      // From 2-9.a we know $\lambda_l(p_l) = 1$; $l = 1,2,3$.
      // Hence we simply \textbf{copy} the coefficient of $\lambda_l$ to $b_l$
      // for $l=1,2,3$.
      zeta(quad_dofs[l]) = mu(lin_dofs[l]);
      // And $\lambda_l(p_l+3) = 0.5$, $\lambda_{(l+1) mod 3}(p_l+3) = 0.5$;
      // $l =1,2,3$. Hence we copy 0.5 of the respective coefficients!
      zeta(quad_dofs[l + 3]) =
          0.5 * (mu(lin_dofs[l]) + mu(lin_dofs[(l + 1) % 3]));
    }
  }
  return zeta;
}
/* SAM_LISTING_END_5 */

}  // namespace LFPPDofHandling


#include "LatticeFitter.hpp"
#include <algorithm>
#include <NTL/LLL.h>

LatticeFitter::LatticeFitter(QObject* parent)
  : QObject(parent) {}

void LatticeFitter::findBestLattice(const std::vector<cv::Point2f>& points)
{
  emit latticeFittingStarted();
  emit progressUpdated(0);

  std::vector<Lattice> bestLattices;

  double progress = 0.0;
  double progress_delta = 100.0 / points.size();

  for (const auto& p : points) {
    bestLattices.emplace_back(bestLatticeForOrigin(p, points));
    progress += progress_delta;
    emit progressUpdated(static_cast<int>(std::round(progress)));
  }

  best_lattice = *std::min_element(bestLattices.cbegin(), bestLattices.cend(),
  [](const Lattice& l1, const Lattice& l2) {
    return l1.total_error < l2.total_error;
  });

  emit foundBestLattice(best_lattice);
}

Lattice LatticeFitter::bestLatticeForOrigin(
    const cv::Point2f& origin, const std::vector<cv::Point2f>& points) const
{
  std::vector<NTL::vec_ZZ> vecs;

  for (const auto& point : points) {
    if (point == origin) continue;

    vecs.push_back(cvPoint2fToNTLVec(point - origin));
  }

  std::vector<Lattice> lattices;

  for (size_t i = 0; i < vecs.size(); ++i) {
    for (size_t j = i + 1; j < vecs.size(); ++j) {
      lattices.emplace_back();
      auto& current_lattice = lattices.back();

      current_lattice.origin = cvPoint2fToNTLVec(origin);
      current_lattice.bases.SetDims(2, 2);
      current_lattice.bases[0] = vecs[i];
      current_lattice.bases[1] = vecs[j];

      NTL::LLL_FP(current_lattice.bases);
      calculateErrorForLattice(current_lattice, vecs);
    }
  }

  return *std::min_element(lattices.cbegin(), lattices.cend(),
  [](const Lattice& l1, const Lattice& l2) {
    return l1.total_error < l2.total_error;
  });
}

void LatticeFitter::calculateErrorForLattice(Lattice& lattice,
    const std::vector<NTL::vec_ZZ>& points) const
{
  lattice.total_error = 0;

  for (const auto& vec : points) {
    NTL::vec_ZZ v;
    NTL::NearVector(v, lattice.bases, vec);
    auto u = v - vec;

    lattice.total_error += std::hypot(NTL::to_double(u[0]), NTL::to_double(u[1]));
  }
}