#pragma once
#ifndef _MATHUTIL_H
#define _MATHUTIL_H

#include <assert.h>

#include <random>
#include <limits>
#include <memory>
#include <iostream>
#include <iomanip>

#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/calib3d.hpp>

namespace mut
{
   const double PI = 3.14159265358979323846;
   const float PI_F = 3.14159265358979f;
   const double NaN = std::numeric_limits<double>::quiet_NaN();
   const double NaN_F = std::numeric_limits<float>::quiet_NaN();

   static Eigen::Quaterniond rotate_between(const Eigen::Vector3d &from, const Eigen::Vector3d &to,
                                            const Eigen::Vector3d &fallbackAxis = Eigen::Vector3d(0, 0, 0))
   //-----------------------------------------------------------------------------------------------
   {
      Eigen::Quaterniond q;
      Eigen::Vector3d v0 = from;
      Eigen::Vector3d v1 = to;
      v0.normalize();
      v1.normalize();

      double d = v0.dot(v1);
      if (d >= 1.0f)
         return Eigen::Quaterniond(1, 0, 0, 0);

      if (d < (1e-6f - 1.0f))
      {
         if (fallbackAxis != Eigen::Vector3d(0, 0, 0))
            q = Eigen::AngleAxis<double>(PI, fallbackAxis);
         else
         {
            // Generate an axis
            Eigen::Vector3d axis = Eigen::Vector3d(1, 0, 0).cross(from);
            if (axis.norm() < 0.000000001) // pick another if colinear
               axis = Eigen::Vector3d(0, 1, 0).cross(from);
            axis.normalize();
            q = Eigen::AngleAxis<double>(PI, axis);
         }
      }
      else
      {
         double s = sqrt((1 + d) * 2);
         double invs = 1 / s;

         Eigen::Vector3d c = v0.cross(v1);

         q.x() = c.x() * invs;
         q.y() = c.y() * invs;
         q.z() = c.z() * invs;
         q.w() = s * 0.5f;
         q.normalize();
      }
      return q;
   }

   template<typename T> static inline bool near_zero(T v, T epsilon);

   template<> bool near_zero(long double v, long double epsilon) { return (fabsl(v) <= epsilon); }

   template<> bool near_zero(double v, double epsilon) { return (fabs(v) <= epsilon); }

   template<> bool near_zero(float v, float epsilon) { return (fabsf(v) <= epsilon); }

   inline static double radiansToDegrees(double radians) { return (radians * 180.0 / PI); }

   inline static float radiansToDegrees(float radians) { return (radians * 180.0f / PI_F); }

   inline static double degreesToRadians(double degrees) { return (degrees * PI / 180.0); }

   inline static float degreesToRadians(float degrees) { return (degrees * PI_F / 180.0f); }

   template<typename T>
   inline static void normalize(T &v)
//-----------------------------------------------
   {
      auto magnitude = v.dot(v);
      if (magnitude > 1.0e-8)
         v /= sqrt(magnitude);
   }

   template<typename T>
   inline static void unchecked_normalize(T &v)
   { v /= sqrt(v.dot(v)); }

   inline static Eigen::Matrix3d skew33d(Eigen::Vector3d v3)
  //-----------------------------------------------
   {
      double a = v3[0], b = v3[1], c = v3[2];
      Eigen::Matrix3d X;
      X << 0, -c, b, c, 0, -a, -b, a, 0;
      return X;
   }

   inline static Eigen::Vector3d solve_homogeneous(Eigen::Matrix3d A)
//---------------------------------------------------------------------------------------------------------------------------
   {
//   std::cout << "Ax = 0:" << std::endl << A << std::endl;
      Eigen::JacobiSVD<Eigen::MatrixXd, Eigen::FullPivHouseholderQRPreconditioner>
            svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
      auto U = svd.matrixU();
      auto V = svd.matrixV();
//   std::cout << "U: " << U << std::endl << "S: " << svd.singularValues() << std::endl << "V: " << V << std::endl;
      return V.col(V.cols() - 1);
   }

   inline static Eigen::Vector3d ray(const Eigen::Matrix3d &KI, double x, double y, double z = 1,
                                                bool is_homogenouous = false, bool is_normalize = false)
//----------------------------------------------------------------------------------
   {
      Eigen::Vector3d v(x, y, z);
      Eigen::Vector3d ray = KI * v;
      if (is_homogenouous)
         ray /= ray[2];
      if (is_normalize)
         ray.normalize();
      return ray;
   }

   inline static Eigen::Vector3d ray(const Eigen::Matrix3d &KI, Eigen::Vector3d v, bool is_homogenouous = false,
                                                bool is_normalize = false)
//--------------------------------------------------------------------------------------------------
   {
      Eigen::Vector3d ray = KI * v;
      if (is_homogenouous)
         ray /= ray[2];
      if (is_normalize)
         ray.normalize();
      //Eigen::Vector3d p  = KI.inverse()*Eigen::Vector3d(1128,592);
      return ray;
   }

   inline static Eigen::Vector3d ray(const Eigen::Matrix3d &KI, cv::Point3d pt, bool is_homogenouous = false,
                                                bool is_normalize = false)
   //-----------------------------------------------------------------------------------------------------------------------
   {
      Eigen::Vector3d v(pt.x, pt.y, pt.z);
      Eigen::Vector3d ray = KI * v;
      if (is_homogenouous)
         ray /= ray[2];
      if (is_normalize)
         ray.normalize();
      return ray;
   }

   inline static void yawpitchroll(const Eigen::Vector3d train, const Eigen::Vector3d query,
                                   double& roll_degrees, double& pitch_degrees, double &yaw_degrees)
   //----------------------------------------------------------------------------------------
   {
      Eigen::Quaterniond Q = rotate_between(query, train);
      Eigen::Matrix3d R = Q.toRotationMatrix();
      Eigen::Vector3d euler = R.eulerAngles(0, 1, 2);
      roll_degrees = radiansToDegrees(euler[0]);
      pitch_degrees = radiansToDegrees(euler[1]);
      yaw_degrees = radiansToDegrees(euler[2]);

      std::cout << std::fixed << std::setprecision(8) << train.transpose() << " -> "  << query.transpose()
                << ": Roll " << mut::radiansToDegrees(euler[0]) << " Pitch " << mut::radiansToDegrees(euler[1])
                << " Yaw " << mut::radiansToDegrees(euler[2]) << std::endl << R << std::endl;

      Q.setFromTwoVectors(query, train);
      R = Q.toRotationMatrix();
      euler = R.eulerAngles(0, 1, 2);
      std::cout << std::fixed << std::setprecision(8) << train.transpose() << " -> "  << query.transpose()
                << ": Roll " << mut::radiansToDegrees(euler[0]) << " Pitch " << mut::radiansToDegrees(euler[1])
                << " Yaw " << mut::radiansToDegrees(euler[2]) << std::endl << R << std::endl;
   }

   static Eigen::Vector3d quaternion_to_euler_angle(Eigen::Quaterniond Q)
   //--------------------------------------------------------------------
   {
      double w = Q.w();
      double x = Q.x();
      double y = Q.y();
      double z = Q.z();

      double ysqr = y * y;

      double t0 = +2.0 * (w * x + y * z);
      double t1 = +1.0 - 2.0 * (x * x + ysqr);
      double X = atan2(t0, t1);

      double t2 = +2.0 * (w * y - z * x);
      t2 = (t2 > +1.0) ? +1.0 : t2;
      t2 = (t2 < -1.0) ? -1.0 : t2;
      double Y = asin(t2);

      double t3 = +2.0 * (w * z + x * y);
      double t4 = +1.0 - 2.0 * (ysqr + z * z);
      double Z = atan2(t3, t4);

      return Eigen::Vector3d(X, Y, Z);
   }

   static cv::Vec3f rotation2Euler(cv::Mat &R)
   //-----------------------------------------
   {
      Eigen::Matrix3d ER;
      cv::cv2eigen(R, ER);
      Eigen::Vector3d euler = ER.eulerAngles(0, 1, 2);
      return cv::Vec3f(euler[0], euler[1], euler[2]);
   }

   static Eigen::Quaterniond euler2Quaternion(const double roll, const double pitch, const double yaw )
   //-------------------------------------------------------------------------------------------
   {
      Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
      Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
      Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
      Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
      return q;
   }

   static cv::Vec3d ocvMultMatVec(const cv::Mat &P, double x, double y, double z)
   //---------------------------------------------------------------------------
   {
      cv::Mat V(1, 3, CV_64FC1);
      double *pV = V.ptr<double>(0);
      pV[0] = x;
      pV[1] = y;
      pV[2] = z;
      cv::Mat R = P * V.t();
      R = R.t();
      pV = R.ptr<double>(0);
      return cv::Vec3d(pV[0], pV[1], pV[2]);
   }

   static cv::Vec3d ocvMultMatVec(const cv::Mat &P, cv::Vec3d v)
   //------------------------------------------------------------
   {
      cv::Mat V(1, 3, CV_64FC1);
      double *pV = V.ptr<double>(0);
      pV[0] = v[0];
      pV[1] = v[1];
      pV[2] = v[2];
      cv::Mat R = P * V.t();
      R = R.t();
      pV = R.ptr<double>(0);
      return cv::Vec3d(pV[0], pV[1], pV[2]);
   }

   static inline cv::Vec3d toOcvRotationVec(Eigen::Quaterniond Q)
   //-----------------------------------------------------
   {
      Eigen::AngleAxisd aa(Q);
      Eigen::Vector3d v = aa.angle()*aa.axis();
/*
      auto R = Q.toRotationMatrix();
      cv::Mat OR;
      cv::eigen2cv(R, OR);
      cv::Vec3d vv;
      cv::Rodrigues(OR, vv);
      std::cout << v.transpose() << " " << vv.t() << std::endl;
*/
      return cv::Vec3d(v[0], v[1], v[2]);
   }

   static inline Eigen::Quaterniond toQuaternion(cv::InputArray m)
   //-----------------------------------------------------
   {
      cv::Mat R;
      cv::Rodrigues(m, R);
      Eigen::Matrix3d ER;
      cv::cv2eigen(R, ER);
      return Eigen::Quaterniond(ER);
   }

   static void decode_quaternion(const Eigen::Quaterniond qin, Eigen::Vector3d& axis, double& angle)
   //-------------------------------------------------------------------------------------
   {
      Eigen::Quaterniond Q = qin;
      if (Q.w() > 1)
         Q.normalize(); // if w>1 acos and sqrt will produce errors, this cant happen if quaternion is normalised
      angle = 2 * acos(Q.w());
      double s = sqrt(1 - Q.w()*Q.w()); // assuming quaternion normalised then w is less than 1, so term always positive.
      if (s >= 0.001)
      { // test to avoid divide by zero, s is always positive due to sqrt
         // if s close to zero then direction of axis not important
         axis[0] = Q.x(); // if it is important that axis is normalised then replace with x=1; y=z=0;
         axis[1] = Q.y();
         axis[2] = Q.z();
      }
      else
      {
         axis[0] = Q.x() / s;
         axis[1] = Q.y() / s;
         axis[2] = Q.z() / s;
         axis.normalize();
      }
   }

   static void essential_decompose(const Eigen::Matrix3d& essential_matrix, Eigen::Matrix3d* rotation1,
                            Eigen::Matrix3d* rotation2, Eigen::Vector3d* translation)
   //------------------------------------------------------------------------------------------
   {
      Eigen::Matrix3d d;
      d << 0, 1, 0, -1, 0, 0, 0, 0, 1;

      const Eigen::JacobiSVD<Eigen::Matrix3d> svd(
            essential_matrix, Eigen::ComputeFullU | Eigen::ComputeFullV);
      Eigen::Matrix3d U = svd.matrixU();
      Eigen::Matrix3d V = svd.matrixV();
      if (U.determinant() < 0) {
         U.col(2) *= -1.0;
      }

      if (V.determinant() < 0) {
         V.col(2) *= -1.0;
      }

      // Possible configurations.
      *rotation1 = U * d * V.transpose();
      *rotation2 = U * d.transpose() * V.transpose();
      *translation = U.col(2).normalized();
   }

   inline double essential_check(const Eigen::Matrix3d R, const double tx, const double ty, const double tz,
                                 const double xt, const double yt, const double xq,  const double yq)
   {
      return  -R(0, 0)*ty*xt - R(0, 1)*ty*yt - R(0, 2)*ty + R(1, 0)*tx*xt + R(1, 1)*tx*yt +
               R(1, 2)*tx + xq*(-R(1, 0)*tz*xt - R(1, 1)*tz*yt - R(1, 2)*tz + R(2, 0)*ty*xt +
               R(2, 1)*ty*yt + R(2, 2)*ty) - yq*(-R(0, 0)*tz*xt - R(0, 1)*tz*yt - R(0, 2)*tz +
               R(2, 0)*tx*xt + R(2, 1)*tx*yt + R(2, 2)*tx);
   }

   static bool check_essential(Eigen::Matrix3d R, Eigen::Vector3d T, double epsilon =0.0000001)
   //------------------------------------------------------------------------------------
   {
      Eigen::Matrix3d E = skew33d(T)*R;
//      std::cout << R << std::endl << E << std::endl;
      if (! near_zero(E.determinant(), epsilon))
         return false;
      Eigen::Matrix3d EET = E*E.transpose();
      Eigen::Matrix3d constraint = 2*EET*E - EET.trace()*E;
//      std::cout << constraint << std::endl;
      struct ZeroVisit
      {
         double total = 0;
         void init(const double& value, Eigen::Index i, Eigen::Index j) {}
         void operator() (const double& value, Eigen::Index i, Eigen::Index j) { total += value; }
      } visitor;
      constraint.visit(visitor);
      if (! near_zero(visitor.total, epsilon))
         return false;

      Eigen::JacobiSVD<Eigen::MatrixXd, Eigen::FullPivHouseholderQRPreconditioner>
            svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
      auto U = svd.matrixU();
      auto V = svd.matrixV();
      auto S = svd.singularValues().asDiagonal().toDenseMatrix();
//      std::cout << "U: " << U << std::endl << "S: " << S << std::endl << "V: " << V << std::endl;
      if (!near_zero(S(2, 2), epsilon))
         return false;
   //   if ( (!near_zero(S(0, 0) - 1, 0.0000001)) || (!near_zero(S(1, 1) - 1, 0.0000001)) )
   //   {
   //      S(0, 0) = S(1, 1) = 1.0;
   //      S(2, 2) = 0.0;
   //      auto A = U*S*V.transpose();
   //      std::cout << A << std::endl;
   //   }
      return true;
   }

   // or just R = Eigen::Quaterniond::UnitRandom().toRotationMatrix();
   static void randomRotationMatrix(std::mt19937* generator, std::uniform_real_distribution<double>* distribution,
                                    Eigen::Matrix3d& R)
//-------------------------------------------------------------------------------------------------------------
   {
      double x = (*distribution)(*generator);
      double y = (*distribution)(*generator);
      double z = (*distribution)(*generator);
      double theta = std::acos(2*x - 1);
      double phi = 2*mut::PI*y;

      double r  = sqrt(z);
      double Vx = sin(phi) * r;
      double Vy = cos(phi) * r;
      double Vz = sqrt(2.f - z);

      double st = sin(theta);
      double ct = cos(theta);
      double Sx = Vx * ct - Vy * st;
      double Sy = Vx * st + Vy * ct;

      R(0,0) = Vx * Sx - ct;
      R(0,1) = Vx * Sy - st;
      R(0,2) = Vx * Vz;
      R(1,0) = Vy * Sx + st;
      R(1,1) = Vy * Sy - ct;
      R(1,2) = Vy * Vz;
      R(2,0) = Vz * Sx;
      R(2,1) = Vz * Sy;
      R(2,2) = 1.0 - z;

      assert(mut::near_zero(R.determinant() - 1.0, 0.000001));
   }

   static void random_vector3d_fixed_norm_kludge(const double norm_sqrt, Eigen::Vector3d& v)
   //-------------------------------------------------------------------------------
   {
      std::random_device rand_device;
      std::mt19937 rand_generator;
      std::unique_ptr<std::uniform_real_distribution<double>> distribution;
      std::uniform_int_distribution<int> index_distr(0,2);
      std::vector<double> pot;

      distribution.reset(new std::uniform_real_distribution<double>(0, norm_sqrt));
      double one = ((*distribution)(rand_generator));
      distribution.reset(new std::uniform_real_distribution<double>(0, std::max(norm_sqrt - one, 0.0)));
      double two = ((*distribution)(rand_generator));
      double three = std::max(norm_sqrt - one - two, 0.0);
      pot.clear();
      pot.push_back(one); pot.push_back(two); pot.push_back(three);
      int i1 = index_distr(rand_generator), i2 = index_distr(rand_generator), i3;
      while (i2 == i1) i2 = index_distr(rand_generator);
      switch (i1)
      {
         case 0: if (i2 == 1) i3 = 2; else i3 = 1; break;
         case 1: if (i2 == 0) i3 = 2; else i3 = 0; break;
         case 2: if (i2 == 0) i3 = 1; else i3 = 0; break;
      }
      v[0] = pot[i1]; v[1] = pot[i2]; v[2] = pot[i3];
   }

/*
int essential_best_pose(const Eigen::Matrix3d& essential_matrix,
                        const std::vector<FeatureCorrespondence>& normalized_correspondences,
                        Eigen::Matrix3d* rotation, Eigen::Vector3d* position)
{
   Eigen::Matrix3d rotation1, rotation2;
   Eigen::Vector3d translation;
   essential_decompose(essential_matrix, &rotation1, &rotation2, &translation);
   const std::vector<Eigen::Matrix3d> rotations = {rotation1, rotation1, rotation2, rotation2};
   const std::vector<Eigen::Vector3d> positions = {
         -rotations[0].transpose() * translation,
         -rotations[1].transpose() * -translation,
         -rotations[2].transpose() * translation,
         -rotations[3].transpose() * -translation};

   // From the 4 candidate poses, find the one with the most triangulated points
   // in front of the camera.
   std::vector<int> points_in_front_of_cameras(4, 0);
   for (int i = 0; i < 4; i++) {
      for (const auto& correspondence : normalized_correspondences) {
         if (IsTriangulatedPointInFrontOfCameras(
               correspondence, rotations[i], positions[i])) {
            ++points_in_front_of_cameras[i];
         }
      }
   }

   // Find the pose with the most points in front of the camera.
   const auto& max_element = std::max_element(points_in_front_of_cameras.begin(),
                                              points_in_front_of_cameras.end());
   const int max_index =
         std::distance(points_in_front_of_cameras.begin(), max_element);

   // Set the pose.
   *rotation = rotations[max_index];
   *position = positions[max_index];
   return *max_element;
}
*/
}
#endif


////////////////////////////////////////////////////////////////////////////////
#include <polyfem/LineQuadrature.hpp>
#include <polyfem/TriQuadrature.hpp>
#include <polyfem/TetQuadrature.hpp>
#include <iostream>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <catch.hpp>
////////////////////////////////////////////////////////////////////////////////

using namespace polyfem;

const double pi = 3.14159265358979323846264338327950288419717;

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

namespace {

// double p01_exact() {
// 	return pi * pi / 6.0;
// }

// Eigen::VectorXd p01_fun(const Eigen::MatrixXd &x) {
// 	Eigen::VectorXd f = 1.0 / (1.0 - x.row(0).array() * x.row(1).array());
// 	return f;
// }

// Eigen::AlignedBox2d p01_lim() {
// 	Eigen::AlignedBox2d box;
// 	box.min().setConstant(0.0);
// 	box.max().setConstant(1.0);
// 	return box;
// }

} // anonymous namespace

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

TEST_CASE("weights", "[quadrature]") {
	// Segment
	for (int order = 1; order <= 64; ++order) {
		LineQuadrature tri;
		Quadrature quadr;
		tri.get_quadrature(order, quadr);
		REQUIRE(quadr.weights.sum() == Approx(1.0).margin(1e-12));
		REQUIRE(quadr.points.minCoeff() >= 0.0);
		REQUIRE(quadr.points.maxCoeff() <= 1.0);
	}

	// Triangle
	for (int order = 1; order < 16; ++order) {
		TriQuadrature tri;
		Quadrature quadr;
		tri.get_quadrature(order, quadr);
		REQUIRE(quadr.weights.sum() == Approx(0.5).margin(1e-12));
		REQUIRE(quadr.points.minCoeff() >= 0.0);
		REQUIRE(quadr.points.maxCoeff() <= 1.0);
	}

	// Tetrahedron
	for (int order = 1; order < 16; ++order) {
		TetQuadrature tri;
		Quadrature quadr;
		tri.get_quadrature(order, quadr);
		REQUIRE(quadr.weights.sum() == Approx(1.0/6.0).margin(1e-12));
		REQUIRE(quadr.points.minCoeff() >= 0.0);
		REQUIRE(quadr.points.maxCoeff() <= 1.0);
	}
}

//TEST_CASE("triangle", "[quadrature]") {
//	for (int order = 1; order < 10; ++order) {
//		Quadrature quadr;
//		TriQuadrature tri;
//		tri.get_quadrature(order, quadr);
//	}
//
//	// REQUIRE(poly_fem::determinant(mat) == Approx(mat.determinant()).margin(1e-12));
//}


#include <Eigen/Dense>
#include "../matplotlibcpp.h"
namespace plt = matplotlibcpp;

template <typename Vector>
std::pair<Eigen::MatrixXd, Eigen::MatrixXd> meshgrid(const Vector& x, const Vector& y) {
  const unsigned m = x.size(), n = y.size();
  // todo fix type
  Eigen::MatrixXd X(m, n), Y(m, n);
  for (unsigned i = 0; i < m; ++i) {
    for (unsigned j = 0; j < n; ++ j) {
      X(i, j) = *(x.data() + i);
      Y(i, j) = *(y.data() + j);
    }
  }
  std::pair<Eigen::MatrixXd, Eigen::MatrixXd> res = std::make_pair(X, Y);
  return res;
}

void plot_contour() {
  const unsigned n = 100;
  Eigen::VectorXd x = Eigen::VectorXd::LinSpaced(n, 0, 1),
                  y = Eigen::VectorXd::LinSpaced(n, 0, 1);
  std::pair<Eigen::MatrixXd, Eigen::MatrixXd> XY = meshgrid(x, y);
  Eigen::MatrixXd Z = XY.first + 2 * XY.second;

  plt::contour(XY.first, XY.second, Z);
  plt::show();

}

int main() {
  plot_contour();
  return 0;
}


#include <cslibs_kdl/residual_vector_kalman.h>
#include <cslibs_kdl/kdl_conversion.h>
#include <Eigen/Dense>
ResidualVectorKalman::ResidualVectorKalman():
    n_(0)
{

}

void ResidualVectorKalman::setDimensions(std::size_t n)
{
    n_ = n;
    eye_ = Eigen::MatrixXd::Identity(n,n);
}
void ResidualVectorKalman::initialize(std::size_t n, const Eigen::VectorXd& state)
{
    state_ = state;
    n_ = n;
    eye_ = Eigen::MatrixXd::Identity(n_,n_);
    cov_ = Eigen::MatrixXd::Identity(n_,n_);
}

void ResidualVectorKalman::initialize(const std::vector<double> &state)
{
    cslibs_kdl::convert(state, state_);
    n_ = state.size();
    eye_ = Eigen::MatrixXd::Identity(n_,n_);
    cov_ = Eigen::MatrixXd::Identity(n_,n_);
}

void ResidualVectorKalman::setCovarianceProcess(const Eigen::MatrixXd& Q)
{
    cov_state_ = Q;
}

void ResidualVectorKalman::setCovarianceMeasurment(const Eigen::MatrixXd &R)
{
    cov_measurment_ = R;
}

Eigen::VectorXd ResidualVectorKalman::update(Eigen::VectorXd& measurement)
{

    Eigen::MatrixXd cov_pri = cov_ + cov_measurment_;

    Eigen::MatrixXd K = cov_pri * (cov_pri + cov_state_).inverse();
    state_ += K *(measurement - state_);
    cov_ = (eye_ - K)*cov_pri;

    return state_;

}

void ResidualVectorKalman::update(const std::vector<double>& measurement, std::vector<double>& result)
{
    Eigen::VectorXd meas;
    cslibs_kdl::convert(measurement, meas);
    Eigen::VectorXd res = update(meas);
    cslibs_kdl::convert(res, result);
}

Eigen::MatrixXd ResidualVectorKalman::getCovariance() const
{
    return cov_;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_RANDOM_NORMAL_DISTRIBUTION_HPP
#define BOOST_COMPUTE_RANDOM_NORMAL_DISTRIBUTION_HPP

#include <limits>

#include <boost/compute/command_queue.hpp>
#include <boost/compute/function.hpp>
#include <boost/compute/types/builtin.hpp>
#include <boost/compute/type_traits/make_vector_type.hpp>

namespace boost {
namespace compute {

/// \class normal_distribution
/// \brief Produces random, normally-distributed floating-point numbers.
///
/// The following example shows how to setup a normal distribution to
/// produce random \c float values centered at \c 5:
///
/// \snippet test/test_normal_distribution.cpp generate
///
/// \see default_random_engine, uniform_real_distribution
template<class RealType = float>
class normal_distribution
{
public:
    typedef RealType result_type;

    /// Creates a new normal distribution producing numbers with the given
    /// \p mean and \p stddev.
    normal_distribution(RealType mean = 0.f, RealType stddev = 1.f)
        : m_mean(mean),
          m_stddev(stddev)
    {
    }

    /// Destroys the normal distribution object.
    ~normal_distribution()
    {
    }

    /// Returns the mean value of the distribution.
    result_type mean() const
    {
        return m_mean;
    }

    /// Returns the standard-deviation of the distribution.
    result_type stddev() const
    {
        return m_stddev;
    }

    /// Returns the minimum value of the distribution.
    result_type min() const
    {
        return -std::numeric_limits<RealType>::infinity();
    }

    /// Returns the maximum value of the distribution.
    result_type max() const
    {
        return std::numeric_limits<RealType>::infinity();
    }

    /// Generates normally-distributed floating-point numbers and stores
    /// them to the range [\p first, \p last).
    template<class OutputIterator, class Generator>
    void generate(OutputIterator first,
                  OutputIterator last,
                  Generator &generator,
                  command_queue &queue)
    {
        typedef typename make_vector_type<RealType, 2>::type RealType2;

        size_t count = detail::iterator_range_size(first, last);

        vector<uint_> tmp(count, queue.get_context());
        generator.generate(tmp.begin(), tmp.end(), queue);

        BOOST_COMPUTE_FUNCTION(RealType2, box_muller, (const uint2_ x),
        {
            const RealType x1 = x.x / (RealType) (UINT_MAX - 1);
            const RealType x2 = x.y / (RealType) (UINT_MAX - 1);

            const RealType z1 = sqrt(-2.f * log2(x1)) * cos(2.f * M_PI_F * x2);
            const RealType z2 = sqrt(-2.f * log2(x1)) * sin(2.f * M_PI_F * x2);

            return (RealType2)(MEAN, MEAN) + (RealType2)(z1, z2) * (RealType2)(STDDEV, STDDEV);
        });

        box_muller.define("MEAN", boost::lexical_cast<std::string>(m_mean));
        box_muller.define("STDDEV", boost::lexical_cast<std::string>(m_stddev));
        box_muller.define("RealType", type_name<RealType>());
        box_muller.define("RealType2", type_name<RealType2>());

        transform(
            make_buffer_iterator<uint2_>(tmp.get_buffer(), 0),
            make_buffer_iterator<uint2_>(tmp.get_buffer(), count / 2),
            make_buffer_iterator<RealType2>(first.get_buffer(), 0),
            box_muller,
            queue
        );
    }

private:
    RealType m_mean;
    RealType m_stddev;
};

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_RANDOM_NORMAL_DISTRIBUTION_HPP


#include <common_robotics_utilities/conversions.hpp>

#include <vector>

#include <Eigen/Geometry>

namespace common_robotics_utilities
{
namespace conversions
{
Eigen::Quaterniond QuaternionFromRPY(const double R,
                                     const double P,
                                     const double Y)
{
  const Eigen::AngleAxisd roll(R, Eigen::Vector3d::UnitX());
  const Eigen::AngleAxisd pitch(P, Eigen::Vector3d::UnitY());
  const Eigen::AngleAxisd yaw(Y, Eigen::Vector3d::UnitZ());
  const Eigen::Quaterniond quat(roll * pitch * yaw);
  return quat;
}

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Quaterniond QuaternionFromUrdfRPY(const double R,
                                         const double P,
                                         const double Y)
{
  const Eigen::AngleAxisd roll(R, Eigen::Vector3d::UnitX());
  const Eigen::AngleAxisd pitch(P, Eigen::Vector3d::UnitY());
  const Eigen::AngleAxisd yaw(Y, Eigen::Vector3d::UnitZ());
  const Eigen::Quaterniond quat(yaw * pitch * roll);
  return quat;
}

// Returns XYZ Euler angles
Eigen::Vector3d EulerAnglesFromRotationMatrix(
    const Eigen::Matrix3d& rot_matrix)
{
  // Use XYZ angles
  const Eigen::Vector3d euler_angles = rot_matrix.eulerAngles(0, 1, 2);
  return euler_angles;
}

// Returns XYZ Euler angles
Eigen::Vector3d EulerAnglesFromQuaternion(const Eigen::Quaterniond& quat)
{
  return EulerAnglesFromRotationMatrix(quat.toRotationMatrix());
}

// Returns XYZ Euler angles
Eigen::Vector3d EulerAnglesFromIsometry3d(const Eigen::Isometry3d& trans)
{
  return EulerAnglesFromRotationMatrix(trans.rotation());
}

Eigen::Isometry3d TransformFromXYZRPY(const double x,
                                      const double y,
                                      const double z,
                                      const double roll,
                                      const double pitch,
                                      const double yaw)
{
  const Eigen::Isometry3d transform
      = Eigen::Translation3d(x, y, z) * QuaternionFromRPY(roll, pitch, yaw);
  return transform;
}

Eigen::Isometry3d TransformFromRPY(const Eigen::Vector3d& translation,
                                   const Eigen::Vector3d& rotation)
{
  const Eigen::Isometry3d transform
      = static_cast<Eigen::Translation3d>(translation)
            * QuaternionFromRPY(rotation.x(), rotation.y(), rotation.z());
  return transform;
}

Eigen::Isometry3d TransformFromRPY(const Eigen::VectorXd& components)
{
  if (components.size() == 6)
  {
    return Eigen::Translation3d(components(0),
                                components(1),
                                components(2))
           * QuaternionFromRPY(components(3),
                               components(4),
                               components(5));
  }
  else
  {
    throw std::invalid_argument(
          "VectorXd source vector is not 6 elements in size");
  }
}

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Isometry3d TransformFromUrdfXYZRPY(const double x,
                                          const double y,
                                          const double z,
                                          const double roll,
                                          const double pitch,
                                          const double yaw)
{
  const Eigen::Isometry3d transform = Eigen::Translation3d(x, y, z)
                                      * QuaternionFromUrdfRPY(roll, pitch, yaw);
  return transform;
}

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Isometry3d TransformFromUrdfRPY(const Eigen::Vector3d& translation,
                                       const Eigen::Vector3d& rotation)
{
  const Eigen::Isometry3d transform
      = static_cast<Eigen::Translation3d>(translation)
          * QuaternionFromUrdfRPY(rotation.x(), rotation.y(), rotation.z());
  return transform;
}

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Isometry3d TransformFromUrdfRPY(const Eigen::VectorXd& components)
{
  if (components.size() == 6)
  {
    return Eigen::Translation3d(components(0),
                                components(1),
                                components(2))
           * QuaternionFromUrdfRPY(components(3),
                                   components(4),
                                   components(5));
  }
  else
  {
    throw std::invalid_argument(
          "VectorXd source vector is not 6 elements in size");
  }
}

Eigen::VectorXd TransformToRPY(const Eigen::Isometry3d& transform)
{
  Eigen::VectorXd components = Eigen::VectorXd::Zero(6);
  const Eigen::Vector3d translation = transform.translation();
  const Eigen::Vector3d rotation
      = EulerAnglesFromRotationMatrix(transform.rotation());
  components << translation, rotation;
  return components;
}

Eigen::Vector3d StdVectorDoubleToEigenVector3d(
    const std::vector<double>& vector)
{
  if (vector.size() == 3)
  {
    return Eigen::Vector3d(vector.at(0), vector.at(1), vector.at(2));
  }
  else
  {
    throw std::invalid_argument(
          "Vector3d source vector is not 3 elements in size");
  }
}

Eigen::VectorXd StdVectorDoubleToEigenVectorXd(
    const std::vector<double>& vector)
{
  Eigen::VectorXd eigen_vector(vector.size());
  for (size_t idx = 0; idx < vector.size(); idx++)
  {
    const double val = vector.at(idx);
    eigen_vector(static_cast<ssize_t>(idx)) = val;
  }
  return eigen_vector;
}

std::vector<double> EigenVector3dToStdVectorDouble(
    const Eigen::Vector3d& point)
{
  return std::vector<double>{point.x(), point.y(), point.z()};
}

std::vector<double> EigenVectorXdToStdVectorDouble(
    const Eigen::VectorXd& eigen_vector)
{
  std::vector<double> vector(static_cast<size_t>(eigen_vector.size()));
  for (size_t idx = 0; idx < vector.size(); idx++)
  {
    const double val = eigen_vector(static_cast<ssize_t>(idx));
    vector.at(idx) = val;
  }
  return vector;
}

// Takes <x, y, z, w> as is the ROS custom!
Eigen::Quaterniond StdVectorDoubleToEigenQuaterniond(
    const std::vector<double>& vector)
{
  if (vector.size() == 4)
  {
    return Eigen::Quaterniond(
        vector.at(3), vector.at(0), vector.at(1), vector.at(2));
  }
  else
  {
    throw std::invalid_argument(
          "Quaterniond source vector is not 4 elements in size");
  }
}

// Returns <x, y, z, w> as is the ROS custom!
std::vector<double> EigenQuaterniondToStdVectorDouble(
    const Eigen::Quaterniond& quat)
{
  return std::vector<double>{quat.x(), quat.y(), quat.z(), quat.w()};
}
}  // namespace conversions
}  // namespace common_robotics_utilities


#include <fmt/core.h>
#include <yaml-cpp/yaml.h>

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <boost/math/quadrature/gauss.hpp>
#include <iostream>
#include <numeric>
#include <optional>
#include <vector>

#include "NumpySaver.hpp"
#include "bsplines.hpp"

using namespace Eigen;
using std::cout, std::endl, std::cerr;

const char* devider = "##########################################";

template <int num_points>
double matrix_B_element(int i, int j, std::vector<double>& knots, Index order) {
  using boost::math::quadrature::gauss;

  if (std::abs((int)i - (int)j) > order) return 0;

  int imax = std::max(i, j);
  int imin = std::min(i, j);

  Index n = knots.size();

  double sum = 0;

  for (int k = std::max(0, imax - 1); k < std::min(imin + order + 1, n - 1);
       k++) {
    auto f = [&](const double& t) {
      auto [values, idx] = bsplines::bsplines(t, knots, order);
      return values[i + order - idx] * values[j + order - idx];
    };
    sum += gauss<double, num_points>::integrate(f, knots[k], knots[k + 1]);
  }

  return sum;
}

template <int num_points>
double matrix_H_element(int i, int j, std::vector<double>& knots, Index order,
                        int l, int z) {
  using boost::math::quadrature::gauss;
  if (std::abs((int)i - (int)j) > order) return 0;

  Index n = knots.size();
  int imax = std::max(i, j);
  int imin = std::min(i, j);

  double sum = 0;

  for (int k = std::max(0, imax - 1); k < std::min(imin + order + 1, n - 1);
       k++) {
    auto f = [&](const double& t) {
      auto [values, idx] = bsplines::bsplines(t, knots, order);
      auto [values_dd, idx_dd] = bsplines::ndx_bsplines(t, knots, order, 2);
      double h_val =
          -values_dd[j - (idx - order)] / 2. +
          (l * (l + 1.) / (t * t * 2.) - z / t) * values[j - (idx - order)];
      return values[i - (idx - order)] * h_val;
      ;
    };

    sum += gauss<double, num_points>::integrate(f, knots[k], knots[k + 1]);
  }
  return sum;
}

template <int num_points>
MatrixXd matrix_B(std::vector<double>& knots, Index order) {
  Index n = knots.size();
  MatrixXd mat(n - 3 + order, n - 3 + order);

  for (int row = 0; row < mat.rows(); row++)
    for (int col = 0; col < mat.cols(); col++)
      mat(row, col) = matrix_B_element<num_points>(
          row - order + 1, col - order + 1, knots, order);

  return mat;
}

template <int num_points>
MatrixXd matrix_H(std::vector<double>& knots, Index order, int l, int z) {
  Index n = knots.size();
  MatrixXd mat(n - 3 + order, n - 3 + order);

  for (int row = 0; row < mat.rows(); row++)
    for (int col = 0; col < mat.cols(); col++)
      mat(row, col) = matrix_H_element<num_points>(
          row - order + 1, col - order + 1, knots, order, l, z);

  return mat;
}

double evaluate_spline(const double& x, const std::vector<double>& knots,
                       const VectorXd& weights, std::size_t order) {
  auto n = knots.size();
  assert(int(n + order) - 3 == weights.size());

  auto [values, index] = bsplines::bsplines(x, knots, order);

  double sum = 0;
  for (std::size_t i = std::max(int(index) - 1, 0);
       i <= std::min(index - 1 + order, n + order - 4); i++)
    sum += values[i + 1 - index] * weights[i];

  return sum;
}

template <int order>
void solve(std::vector<double>& knots, std::string& basefilename,
           std::optional<double> plot_max, int l, int z) {
  constexpr int gauss_points = order * 1.5;
  int n_knots = knots.size();

  cout << "knots = ";
  for (auto k : knots) cout << k << ", ";
  cout << endl;

  auto B = matrix_B<gauss_points>(knots, order);
  auto H = matrix_H<gauss_points>(knots, order, l, z);

  if (knots.size() < 20) {
    cout << "B=\n" << B << "\n\n";
    cout << "H=\n" << H << endl;
  }

  GeneralizedEigenSolver<MatrixXd> ges;
  ges.compute(H, B);
  VectorXcd eigenvalues = ges.eigenvalues();
  VectorXd real_eigen = eigenvalues.real();

  // sort the eigenvalues (and create an index list)
  std::vector<int> indices(real_eigen.size());
  std::iota(indices.begin(), indices.end(), 0);

  std::sort(indices.begin(), indices.end(), [&](int A, int B) -> bool {
    return real_eigen[A] < real_eigen[B];
  });
  std::sort(real_eigen.data(), real_eigen.data() + real_eigen.size());

  if (eigenvalues.imag().cwiseAbs().sum() > 10e-10) {
    cerr << "Error: Complex eigenvalues!" << endl;
  }

  cout << "Eigenvalues: " << real_eigen.transpose() << endl;

  MatrixXd eigenvectors = ges.eigenvectors().real();

  NumpySaver(fmt::format("build/output/{}_eigenvalues.npy", basefilename))
      << real_eigen;
  NumpySaver(fmt::format("build/output/{}_knots.npy", basefilename)) << knots;

  NumpySaver eigvec_saver(
      fmt::format("build/output/{}_eigenvectors.npy", basefilename));
  for (int col = 0; col < eigenvectors.cols(); col++)
    eigvec_saver << eigenvectors.col(indices[col]);

  if (auto xmax = plot_max) {
    ArrayXd x = ArrayXd::LinSpaced(1000, 0, *xmax);
    MatrixXd points(x.size(), eigenvectors.cols());
    for (int row = 0; row < x.size(); row++)
      for (int col = 0; col < points.cols(); col++)
        points(row, col) = evaluate_spline(
            x[row], knots, eigenvectors.col(indices[col]), order);

    NumpySaver plot_saver(
        fmt::format("build/output/{}_plot_points.npy", basefilename));
    plot_saver << x;
    for (int col = 0; col < points.cols(); col++) plot_saver << points.col(col);
  }
}

template <int order>
void solve_exp(int n_knots, double rmin, double rmax, double exp_fak,
               std::string& basefilename, std::optional<double> plot, int l,
               int z) {
  std::vector<double> knots(n_knots);
  for (int i = 0; i < n_knots; i++)
    knots[i] = (rmax - rmin) *
                   (std::exp(double(i) / double(n_knots - 1) * exp_fak) - 1.) /
                   (std::exp(exp_fak) - 1) +
               rmin;

  solve<order>(knots, basefilename, plot, l, z);
}

template <int order>
void solve_lin(int n_knots, double rmin, double rmax, std::string& basefilename,
               std::optional<double> plot, int l, int z) {
  std::vector<double> knots(n_knots);
  for (int i = 0; i < n_knots; i++)
    knots[i] = double(i) / double(n_knots - 1) * (rmax - rmin) + rmin;

  solve<order>(knots, basefilename, plot, l, z);
}

int main() {
  YAML::Node configs = YAML::LoadFile("config.yaml");

  constexpr int order = 3;

  for (std::size_t i = 0; i < configs.size(); i++) {
    YAML::Node config = configs[i];

    std::optional<double> plot;

    if (auto p_node = config["plot_xmax"]) {
      try {
        auto val = p_node.as<double>();
        if (val > 0) plot.emplace(val);
      } catch (const YAML::TypedBadConversion<double>&) {
        auto boolval = p_node.as<bool>();
        if (boolval) plot.emplace(10);
      }
    }

    auto l = config["l"].as<int>();
    auto z = config["Z"].as<int>();
    auto basefilename = config["name"].as<std::string>();

    fmt::print("\n\n{0}\n# Simulation '{1}' {2}/{3}\n{0}\n", devider,
               basefilename, i + 1, configs.size());

    YAML::Node knot_node = config["knots"];
    auto type = knot_node.Type();
    // single value
    if (type == YAML::NodeType::Sequence) {
      auto knots = knot_node.as<std::vector<double>>();
      solve<order>(knots, basefilename, plot, l, z);
    } else if (type == YAML::NodeType::Map) {
      auto rmin = knot_node["rmin"].as<double>();
      auto rmax = knot_node["rmax"].as<double>();
      auto N = knot_node["N"].as<size_t>();
      auto type = knot_node["type"].as<std::string>();
      if (type == "linear") {
        solve_lin<order>(N, rmin, rmax, basefilename, plot, l, z);
      } else if (type == "exponential") {
        auto expfac = knot_node["expfac"].as<double>();
        solve_exp<order>(N, rmin, rmax, expfac, basefilename, plot, l, z);
      } else {
        throw std::runtime_error(
            "'type' of knots must either be 'linear' or 'exponential'.");
      }
    } else {
      throw std::runtime_error(
          "Range node must either be list, scalar or a map defining "
          "'type', 'N', 'rmin' and 'rmax'.");
    }
  }
}


/** Test cases for Segment class.
 *
 */

#include "utils/random.h"

#ifndef LINK_STATIC
#define BOOST_TEST_DYN_LINK
#endif
#define BOOST_TEST_MODULE RandomTest
#include <boost/test/unit_test.hpp>

#include <iostream>
#include <random>
#include <map>
#include <string>
#include <iomanip>

using namespace utils::random;
using namespace std;

class RandomAnalyzer {
public:
    RandomAnalyzer (const std::vector<double>& random_numbers) :
    m_random_numbers(random_numbers) {
        analyze();
    }

public:
    double max () { return m_max; }
    double min () { return m_min; }
    double mean () { return m_mean; }
    double std_dev () { return m_std_dev; }
    size_t N () { return m_N; }

    double distribution_error(double expected_frequency) {
        std::vector<double> freqs (m_hist.size(), expected_frequency);
        return distribution_error(freqs);
    }

    double distribution_error(const std::vector<double>& expected_frequency) {
        assert (m_hist.size() == expected_frequency.size());
        double dist_error = 0;

        double span = m_max - m_min;
        for (int i = 1; i < m_hist.size() - 1; i += 1) {
            int x = i + m_min;
            cout << "freq[" << i << "] = " << m_hist[x] << endl;
            cout << "expected_freq[" << i << "] = " << expected_frequency[i] << endl;
            dist_error += std::abs((m_hist[x] - expected_frequency[i])
                          / expected_frequency[i]*100);
        }
        dist_error /= span;
        cout << "Dist error: " << dist_error << endl;

        return dist_error;
    }

private:
    void analyze() {
        m_N = m_random_numbers.size();
        m_max = utils::stds::max(m_random_numbers);
        m_min = utils::stds::min(m_random_numbers);
        m_mean = utils::stds::mean(m_random_numbers);
        m_std_dev = utils::stds::std_dev(m_random_numbers);

        for (int i = m_min; i <= m_max; i += 1) {
            m_hist[i] = 0;
        }

        for (int n = 0; n < m_N; n += 1) {
            m_hist[std::round(m_random_numbers[n])] += 1;
        }
    }

private:
    std::vector<double> m_random_numbers;
    size_t m_N = 0;
    double m_min = std::nan("NaN");
    double m_max = std::nan("NaN");
    double m_mean = std::nan("NaN");
    double m_std_dev = std::nan("NaN");
    std::map<int, int> m_hist;
};

BOOST_AUTO_TEST_CASE(UniformRandom_generates_uniform_distribution)
{
    int N = 1000000;
    double min = -8, max = 12, span;
    // While calculating histogram, -8 to -7.5 will be assigned to freq[-8]
    // similarly, 11.5 to 12 will be assigned to 12. Thus, -8 and 12 will
    // have half of actual frequency.
    span = max - min;
    double expected_freq = N/span;

    UniformRandom rnd_generator (min, max);
    std::vector<double> random_numbers;
    random_numbers.reserve(N);
    for (int i = 0; i < N; i += 1) {
        random_numbers.push_back(rnd_generator.generate());
    }

    RandomAnalyzer analyzer (random_numbers);
    BOOST_CHECK_EQUAL(std::round(analyzer.min()), min);
    BOOST_CHECK_EQUAL(std::round(analyzer.max()), max);
    BOOST_CHECK(analyzer.distribution_error(expected_freq) < 1);
}

BOOST_AUTO_TEST_CASE(Random_returns_a_vector_of_numbers)
{
    size_t N = 1000000;
    double min = -8, max = 12;

    UniformRandom uniform_random (min, max);
    auto random_numbers = uniform_random.generate(N);
    RandomAnalyzer analyzer (random_numbers);

    BOOST_REQUIRE_EQUAL(random_numbers.size(), N);

    double expected_average = (max+min)/2;
    BOOST_CHECK_CLOSE(expected_average, analyzer.mean(), 1);
}

BOOST_AUTO_TEST_CASE(NormalRandom_generates_random_numbers_with_correct_stddev_and_mean)
{
    size_t N = 1000000;
    double std_dev = 10, mean = 5;

    NormalRandom random (std_dev, mean);
    auto random_numbers = random.generate(N);
    RandomAnalyzer analyzer (random_numbers);

    BOOST_REQUIRE_EQUAL(random_numbers.size(), N);

    BOOST_CHECK_CLOSE(mean, analyzer.mean(), 1);
    BOOST_CHECK_CLOSE(std_dev, analyzer.std_dev(), 1);
}

BOOST_AUTO_TEST_CASE(NormalRandom_generates_random_numbers_with_correct_min_max)
{
    size_t N = 1000000;
    double std_dev = 10, mean = 0;
    double min = -10, max = 20;

    NormalRandom random (std_dev, mean, min, max);
    auto random_numbers = random.generate(N);
    RandomAnalyzer analyzer (random_numbers);

    BOOST_REQUIRE_EQUAL(random_numbers.size(), N);

    BOOST_CHECK_CLOSE(analyzer.min(), min, 1);
    BOOST_CHECK_CLOSE(analyzer.max(), max, 1);
}

BOOST_AUTO_TEST_CASE(DiscreteRandom_generates_uniform_distribution_with_given_weights)
{
    int N = 1000000;
    double min = -8, max = 11, span;
    span = max - min + 1;
    double expected_freq = N/span;

    std::vector<double> domain = utils::stds::linspace(min, max, size_t(span));
    std::vector<double> weights (span, expected_freq);

    DiscreteRandom rnd_generator (domain, weights);
    std::vector<double> random_numbers = rnd_generator.generate(N);

    RandomAnalyzer analyzer (random_numbers);
    BOOST_CHECK_EQUAL(std::round(analyzer.min()), min);
    BOOST_CHECK_EQUAL(std::round(analyzer.max()), max);
    BOOST_CHECK(analyzer.distribution_error(expected_freq) < 1);
}

BOOST_AUTO_TEST_CASE(DiscreteRandom_generates_uniform_distribution_with_given_weight_function)
{
    struct UniformWeight : public DiscreteRandom::WeightFunction {
        UniformWeight (double weight) : m_weight(weight) {}
        double operator () (const double& value) const {
            return m_weight;
        }

        double m_weight;
    };

    int N = 1000000;
    double min = -8, max = 11, span;
    span = max - min + 1;
    double expected_freq = N/span;

    std::vector<double> domain = utils::stds::linspace(min, max, size_t(span));

    DiscreteRandom rnd_generator (domain, UniformWeight(expected_freq));
    std::vector<double> random_numbers = rnd_generator.generate(N);

    RandomAnalyzer analyzer (random_numbers);
    BOOST_CHECK_EQUAL(std::round(analyzer.min()), min);
    BOOST_CHECK_EQUAL(std::round(analyzer.max()), max);
    BOOST_CHECK(analyzer.distribution_error(expected_freq) < 1);
}

BOOST_AUTO_TEST_CASE(DiscreteRandom_generates_uniform_distribution_with_given_min_max_count)
{
    struct UniformWeight : public DiscreteRandom::WeightFunction {
        UniformWeight (double weight) : m_weight(weight) {}
        double operator () (const double& value) const {
            return m_weight;
        }

        double m_weight;
    };

    int N = 1000000;
    double min = -8, max = 11, span;
    span = max - min + 1;
    double expected_freq = N/span;

    DiscreteRandom rnd_generator (min, max, size_t(span), UniformWeight(expected_freq));
    std::vector<double> random_numbers = rnd_generator.generate(N);

    RandomAnalyzer analyzer (random_numbers);
    BOOST_CHECK_EQUAL(std::round(analyzer.min()), min);
    BOOST_CHECK_EQUAL(std::round(analyzer.max()), max);
    BOOST_CHECK(analyzer.distribution_error(expected_freq) < 1);
}

BOOST_AUTO_TEST_CASE(DiscreteCosineRandom_generates_cosine_distribution_with_given_min_max_count)
{
    int N = 1000000;
    double min = -pi/2, max = pi/2;
    size_t count = 100;

    auto domain = utils::stds::linspace(min, max, count);
    DiscreteCosineRandom rnd_generator (domain);
    std::vector<double> random_numbers = rnd_generator.generate(N);

    RandomAnalyzer analyzer (random_numbers);
    BOOST_CHECK_CLOSE(analyzer.min(), min, 5);
    BOOST_CHECK_CLOSE(analyzer.max(), max, 5);
    BOOST_CHECK_EQUAL(std::round(analyzer.mean()), 0);
    //BOOST_CHECK(analyzer.distribution_error(expected_freq) < 1);
}


#include <iostream>
#include <cmath>

#include <capnp/message.h>
#include <capnp/serialize-packed.h>
#include <Eigen/Dense>

#include "cereal/gen/cpp/log.capnp.h"

#include "locationd_yawrate.h"

void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas) {
  double dt = current_time - prev_update_time;

  if (dt < 0) {
    dt = 0;
  } else {
    prev_update_time = current_time;
  }

  x = A * x;
  P = A * P * A.transpose() + dt * Q;

  double y = meas - C * x;
  double S = R + C * P * C.transpose();
  Eigen::Vector2d K = P * C.transpose() * (1.0 / S);
  x = x + K * y;
  P = (I - K * C) * P;
}

void Localizer::handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time) {
  for (cereal::SensorEventData::Reader sensor_event : sensor_events){
    if (sensor_event.getSensor() == 5 && sensor_event.getType() == 16) {
      sensor_data_time = current_time;
      double meas = -sensor_event.getGyroUncalibrated().getV()[0];
      update_state(C_gyro, R_gyro, current_time, meas);
    }
  }
}

void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) {
  double R = pow(5 * camera_odometry.getRotStd()[2], 2);
  double meas = camera_odometry.getRot()[2];
  update_state(C_posenet, R, current_time, meas);

  auto trans = camera_odometry.getTrans();
  posenet_speed = sqrt(trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2]);
  camera_odometry_time = current_time;
}

void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) {
  steering_angle = controls_state.getAngleSteers() * DEGREES_TO_RADIANS;
  car_speed = controls_state.getVEgo();
  controls_state_time = current_time;
}

Localizer::Localizer() {
  // States: [yaw rate, gyro bias]
  A <<
    1, 0,
    0, 1;

  Q <<
    pow(.1, 2.0), 0,
    0, pow(0.05/ 100.0, 2.0),
  P <<
    pow(10000.0, 2.0), 0,
    0, pow(10000.0, 2.0);

  I <<
    1, 0,
    0, 1;

  C_posenet << 1, 0;
  C_gyro << 1, 1;
  x << 0, 0;

  R_gyro = pow(0.025, 2.0);
}

void Localizer::handle_log(cereal::Event::Reader event) {
  double current_time = event.getLogMonoTime() / 1.0e9;

  // Initialize update_time on first update
  if (prev_update_time < 0) {
    prev_update_time = current_time;
  }

  auto type = event.which();
  switch(type) {
  case cereal::Event::CONTROLS_STATE:
    handle_controls_state(event.getControlsState(), current_time);
    break;
  case cereal::Event::CAMERA_ODOMETRY:
    handle_camera_odometry(event.getCameraOdometry(), current_time);
    break;
  case cereal::Event::SENSOR_EVENTS:
    handle_sensor_events(event.getSensorEvents(), current_time);
    break;
  default:
    break;
  }
}

extern "C" {
  void *localizer_init(void) {
    Localizer * localizer = new Localizer;
    return (void*)localizer;
  }

  void localizer_handle_log(void * localizer, const unsigned char * data, size_t len) {
    const kj::ArrayPtr<const capnp::word> view((const capnp::word*)data, len);
    capnp::FlatArrayMessageReader msg(view);
    cereal::Event::Reader event = msg.getRoot<cereal::Event>();

    Localizer * loc = (Localizer*) localizer;
    loc->handle_log(event);
  }

  double localizer_get_yaw(void * localizer) {
    Localizer * loc = (Localizer*) localizer;
    return loc->x[0];
  }
  double localizer_get_bias(void * localizer) {
    Localizer * loc = (Localizer*) localizer;
    return loc->x[1];
  }

  double * localizer_get_state(void * localizer) {
    Localizer * loc = (Localizer*) localizer;
    return loc->x.data();
  }

  void localizer_set_state(void * localizer, double * state) {
    Localizer * loc = (Localizer*) localizer;
    memcpy(loc->x.data(), state, 4 * sizeof(double));
  }

  double localizer_get_t(void * localizer) {
    Localizer * loc = (Localizer*) localizer;
    return loc->prev_update_time;
  }

  double * localizer_get_P(void * localizer) {
    Localizer * loc = (Localizer*) localizer;
    return loc->P.data();
  }

  void localizer_set_P(void * localizer, double * P) {
    Localizer * loc = (Localizer*) localizer;
    memcpy(loc->P.data(), P, 16 * sizeof(double));
  }
}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_math/blob/master/LICENSE

#ifndef CBR_MATH__INTERP__SPLINE_HPP_
#define CBR_MATH__INTERP__SPLINE_HPP_

#include <Eigen/Dense>

#include <utility>
#include <vector>
#include <array>

#include "piecewise_poly.hpp"

namespace cbr
{

class Spline
{
protected:
  using row_t = Eigen::Matrix<double, 1, Eigen::Dynamic>;
  using matrix_t = Eigen::MatrixXd;

public:
  /**
   * @brief Create a scalar-valued spline
   *
   * @tparam T1 PiecewisePoly::row_t
   * @tparam T2 PiecewisePoly::matrix_t
   * @param x breakpoints of shape 1xN
   * @param y values of shape DxN where D \in {1, 2, 3}
   *
   * D=1: y contains values
   * D=2: y contains values, 1st derivatives
   * D=3: y contains values, 1st derivatives, 2nd derivatives
   */
  template<typename T1, typename T2>
  static PiecewisePoly fit(
    T1 && x,
    const Eigen::DenseBase<T2> & y)
  {
    static_assert(is_eigen_dense_v<T1>, "x must be an Eigen::DenseBase object.");

    if (x.size() < 2) {
      throw std::invalid_argument("x be of size > 1");
    }

    if (x.size() != y.cols()) {
      throw std::invalid_argument("The number of columns of ys must be equal to the size of x.");
    }

    if (!(y.rows() > 0 && y.rows() < 4)) {
      throw std::invalid_argument("y must have between 1 and 3 rows.");
    }

    matrix_t coeffs = generateCoeffs(x, y);

    if (y.rows() == 1 && x.size() == 3) {
      Eigen::Matrix<double, 1, 2> bp{x[0], x[2]};
      return PiecewisePoly(std::move(bp), std::move(coeffs));
    } else {
      return PiecewisePoly(std::forward<T1>(x), std::move(coeffs));
    }
  }

  /**
   * @brief Create a vector-valued spline
   *
   * @tparam T1 PiecewisePoly::row_t
   * @tparam T2 container_t<PiecewisePoly::matrix_t>
   * @param x breakpoints of shape 1xN
   * @param y values of shape MxDxN where M is number of value dimensions and D \in {1, 2, 3}
   *
   * D=1: y[m] contains values for m:th dimension
   * D=2: y[m] contains values, 1st derivatives for m:th dimension
   * D=3: y[m] contains values, 1st, 2nd derivatives for m:th dimension
   */
  template<typename T1, typename T2>
  static PiecewisePolyND fitND(
    T1 && x,
    const T2 & ys)
  {
    static_assert(is_eigen_dense_v<T1>, "x must be an Eigen::DenseBase object.");

    if (x.size() < 2) {
      throw std::invalid_argument("x must be of size > 1");
    }

    if constexpr (is_eigen_dense_v<T2>) {
      if (ys.rows() < 1) {
        throw std::invalid_argument("Number of rows of ys must be > 0.");
      }

      if (x.size() != ys.cols()) {
        throw std::invalid_argument("The number of columns of ys must be equal to the size of x.");
      }

      std::vector<matrix_t> coefLists;
      coefLists.reserve(static_cast<std::size_t>(ys.rows()));

      for (Eigen::Index i = 0; i < ys.rows(); i++) {
        coefLists.push_back(generateCoeffs(x, ys.row(i)));
      }

      return PiecewisePolyND(std::forward<T1>(x), std::move(coefLists));
    } else {
      static_assert(
        is_eigen_dense_v<typename T2::value_type>,
        "ys must be a container of Eigen::DenseBase objects");

      if (ys.size() < 1) {
        throw std::invalid_argument("Number of elements of ys must be > 0.");
      }

      const auto ni = ys[0].rows();
      if (!(ni > 0 && ni < 4)) {
        throw std::invalid_argument("Each element of ys must have between 1 and 3 rows.");
      }
      for (const auto & y : ys) {
        if (y.rows() != ni) {
          throw std::invalid_argument(
                  "Each element of ys must have the same number of rows.");
        }
      }

      std::vector<matrix_t> coefLists;
      coefLists.reserve(ys.size());
      for (const auto & y : ys) {
        coefLists.push_back(generateCoeffs(x, y));
      }

      if (ni == 1 && x.size() == 3) {
        Eigen::Matrix<double, 1, 2> bp{x[0], x[2]};
        return PiecewisePolyND(std::move(bp), std::move(coefLists));
      } else {
        return PiecewisePolyND(std::forward<T1>(x), std::move(coefLists));
      }
    }
  }

protected:
  template<typename T1, typename T2>
  static matrix_t generateCoeffs(
    const Eigen::DenseBase<T1> & x,
    const Eigen::DenseBase<T2> & y)
  {
    if (x.size() != y.cols()) {
      throw std::invalid_argument(
              "The number of columns of each element of ys must be equal to the size of x.");
    }

    matrix_t coeffs;

    const Eigen::Index nx = x.size();
    const Eigen::Index nj = nx - 1;
    const Eigen::Index ni = y.rows();

    if (ni == 1) {
      if (nx == 2) {  // If only 2 points, linear interpolation
        coeffs.resize(2, 1);
        coeffs(1, 0) = y(0, 0);
        coeffs(0, 0) = (y(0, 1) - y(0, 0)) / (x[1] - x[0]);

      } else if (nx == 3) {  // If only 3 points, quadratic interpolation
        coeffs.resize(3, 1);

        Eigen::Matrix3d A = Eigen::Matrix3d::Zero();
        Eigen::Vector3d b = Eigen::Vector3d::Zero();

        A(0, 2) = 1.;

        A(1, 0) = powFast<2>(x[1] - x[0]);
        A(1, 1) = (x[1] - x[0]);
        A(1, 2) = 1.;

        A(2, 0) = powFast<2>(x[2] - x[0]);
        A(2, 1) = (x[2] - x[0]);
        A(2, 2) = 1.;

        b[0] = y(0, 0);
        b[1] = y(0, 1);
        b[2] = y(0, 2);

        Eigen::FullPivLU<Eigen::Matrix3d> dec(A);
        coeffs = dec.solve(b);

      } else {  // If more than 3 points, piecewise cubic interpolation
        coeffs.resize(4, nj);
        Eigen::MatrixXd A = Eigen::MatrixXd::Zero(4 * nj, 4 * nj);
        Eigen::VectorXd b = Eigen::VectorXd::Zero(4 * nj);

        // Function continuity
        Eigen::Index i0 = 0;
        for (Eigen::Index i = 0; i < nx - 1; i++) {
          A(2 * i, 4 * i) = powFast<3>(x[i + 1] - x[i]);
          A(2 * i, 4 * i + 1) = powFast<2>(x[i + 1] - x[i]);
          A(2 * i, 4 * i + 2) = (x[i + 1] - x[i]);
          A(2 * i, 4 * i + 3) = 1.;
          A(2 * i + 1, 4 * i + 3) = 1.;

          b[2 * i] = y(0, i + 1);
          b[2 * i + 1] = y(0, i);
        }

        // First derivative continuity
        i0 += 2 * (nx - 1);
        for (Eigen::Index i = 0; i < nx - 2; i++) {
          A(i0 + i, 4 * i) = 3. * powFast<2>(x[i + 1] - x[i]);
          A(i0 + i, 4 * i + 1) = 2. * (x[i + 1] - x[i]);
          A(i0 + i, 4 * i + 2) = 1.;
          A(i0 + i, 4 * i + 6) = -1.;
        }

        // Second derivatives continuity
        i0 += (nx - 2);
        for (Eigen::Index i = 0; i < nx - 2; i++) {
          A(i0 + i, 4 * i) = 6. * (x[i + 1] - x[i]);
          A(i0 + i, 4 * i + 1) = 2.;
          A(i0 + i, 4 * i + 5) = -2.;
        }

        // Not-a-knot conditions
        i0 += (nx - 2);
        A(i0, 0) = 6.;
        A(i0, 4) = -6.;
        A(i0 + 1, 4 * (nx - 3)) = 6.;
        A(i0 + 1, 4 * (nx - 3) + 4) = -6.;

        // Solve linear system
        Eigen::FullPivLU<Eigen::MatrixXd> dec(A);
        Eigen::Map<Eigen::VectorXd> coeffsMap(coeffs.data(), 4 * nj);
        coeffsMap = dec.solve(b);
      }

    } else if (ni == 2) {
      coeffs.resize(4, nj);
      for (Eigen::Index i = 0; i < nj; i++) {
        Eigen::Matrix4d A = Eigen::Matrix4d::Zero();
        Eigen::Vector4d b = Eigen::Vector4d::Zero();

        // Function continuity
        A(0, 0) = powFast<3>((x[i + 1] - x[i]));
        A(0, 1) = powFast<2>((x[i + 1] - x[i]));
        A(0, 2) = (x[i + 1] - x[i]);
        A(0, 3) = 1.;
        A(1, 3) = 1.;
        b[0] = y(0, i + 1);
        b[1] = y(0, i);

        // First derivatives continuity
        A(2, 0) = 3. * powFast<2>((x[i + 1] - x[i]));
        A(2, 1) = 2. * (x[i + 1] - x[i]);
        A(2, 2) = 1.;
        A(3, 2) = 1.;
        b[2] = y(1, i + 1);
        b[3] = y(1, i);

        // Solve linear system
        Eigen::FullPivLU<Eigen::MatrixXd> dec(A);
        coeffs.col(i) = dec.solve(b);
      }

    } else if (ni == 3) {
      coeffs.resize(6, nj);
      for (Eigen::Index i = 0; i < nj; i++) {
        Eigen::Matrix<double, 6, 6> A = Eigen::Matrix<double, 6, 6>::Zero();
        Eigen::Matrix<double, 6, 1> b = Eigen::Matrix<double, 6, 1>::Zero();

        // Function continuity
        A(0, 0) = powFast<5>((x[i + 1] - x[i]));
        A(0, 1) = powFast<4>((x[i + 1] - x[i]));
        A(0, 2) = powFast<3>((x[i + 1] - x[i]));
        A(0, 3) = powFast<2>((x[i + 1] - x[i]));
        A(0, 4) = (x[i + 1] - x[i]);
        A(0, 5) = 1.;
        A(1, 5) = 1.;
        b[0] = y(0, i + 1);
        b[1] = y(0, i);

        // First derivatives continuity
        A(2, 0) = 5. * powFast<4>((x[i + 1] - x[i]));
        A(2, 1) = 4. * powFast<3>((x[i + 1] - x[i]));
        A(2, 2) = 3. * powFast<2>((x[i + 1] - x[i]));
        A(2, 3) = 2. * (x[i + 1] - x[i]);
        A(2, 4) = 1.;
        A(3, 4) = 1.;
        b[2] = y(1, i + 1);
        b[3] = y(1, i);

        // Second derivatives continuity
        A(4, 0) = 20. * powFast<3>((x[i + 1] - x[i]));
        A(4, 1) = 12. * powFast<2>((x[i + 1] - x[i]));
        A(4, 2) = 6. * (x[i + 1] - x[i]);
        A(4, 3) = 2.;
        A(5, 3) = 2.;
        b[4] = y(2, i + 1);
        b[5] = y(2, i);

        // Solve linear system
        Eigen::FullPivLU<Eigen::Matrix<double, 6, 6>> dec(A);
        coeffs.col(i) = dec.solve(b);
      }

    } else {
      throw std::invalid_argument("y as an unsupported number of rows. Should never happen!");
    }
    return coeffs;
  }
};

}  // namespace cbr

#endif  // CBR_MATH__INTERP__SPLINE_HPP_


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

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

#include <iostream>
using std::cout; using std::endl;

// Setting (approximate) precision 25 bits in a single function call using make_policy.

//[policy_ref_snip11

#include <boost/math/distributions/normal.hpp>
using boost::math::normal_distribution;

using namespace boost::math::policies;

const int bits = 25; // approximate precision.

double q = quantile(
      normal_distribution<double, policy<digits2<bits> > >(),
      0.05); // 5% quantile.

//] //[/policy_ref_snip11]

int main()
{
  std::streamsize p = 2 + (bits * 30103UL) / 100000UL;
  // Approximate number of significant decimal digits for 25 bits.
  cout.precision(p);
  cout << bits << " binary bits is approoximately equivalent to " << p << " decimal digits " << endl;
  cout << "quantile(normal_distribution<double, policy<digits2<25> > >(), 0.05  = "
     << q << endl; // -1.64485
}

/*
Output:
  25 binary bits is approoximately equivalent to 9 decimal digits
  quantile(normal_distribution<double, policy<digits2<25> > >(), 0.05  = -1.64485363
  */


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_ARITHMETIC_CROSS_PRODUCT_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ARITHMETIC_CROSS_PRODUCT_HPP

#include <cstddef>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/geometries/concepts/point_concept.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename P1, typename P2, std::size_t Dimension>
struct cross_product
{
    // We define cross product only for 2d (see Wolfram) and 3d.
    // In Math, it is also well-defined for 7-dimension.
    // Generalisation of cross product to n-dimension is defined as
    // wedge product but it is not direct analogue to binary cross product.
};

template <typename P1, typename P2>
struct cross_product<P1, P2, 2>
{
    typedef P1 return_type;

    static inline return_type apply(P1 const& p1, P2 const& p2)
    {
        assert_dimension<P1, 2>();
        assert_dimension<P2, 2>();

        // For 2-dimensions, analog of the cross product U(x,y) and V(x,y) is
        // Ux * Vy - Uy * Vx
        // which is returned as 0-component (or X) of 2d vector, 1-component is undefined.
        return_type v;
        set<0>(v, get<0>(p1) * get<1>(p2) - get<1>(p1) * get<0>(p2));
        return v;
    }
};

template <typename P1, typename P2>
struct cross_product<P1, P2, 3>
{
    typedef P1 return_type;

    static inline return_type apply(P1 const& p1, P2 const& p2)
    {
        assert_dimension<P1, 3>();
        assert_dimension<P2, 3>();

        return_type v;
        set<0>(v, get<1>(p1) * get<2>(p2) - get<2>(p1) * get<1>(p2));
        set<1>(v, get<2>(p1) * get<0>(p2) - get<0>(p1) * get<2>(p2));
        set<2>(v, get<0>(p1) * get<1>(p2) - get<1>(p1) * get<0>(p2));
        return v;
    }
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

// TODO: This is a simple draft. If relevant, it can be extended to:
// - accept vectors of different coordinate_type, but common coordinate_system
// - if vectors are of mixed 2d and 3d, lower dimension is used
// - define result_type that will generate type of vector based on:
// -- select_coordinate_type
// -- selection of lower dimension

/*!
\brief Computes the cross product of two vectors.
\details Both vectors shall be of the same type.
         This type also determines type of result vector.
\ingroup arithmetic
\param p1 first vector
\param p2 second vector
\return the cross product vector
 */
template <typename P1, typename P2>
inline P1 cross_product(P1 const& p1, P2 const& p2)
{
    BOOST_CONCEPT_ASSERT( (concept::ConstPoint<P1>) );
    BOOST_CONCEPT_ASSERT( (concept::ConstPoint<P2>) );

    return detail::cross_product
        <
            P1, P2,
            dimension<P1>::type::value
        >::apply(p1, p2);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_ARITHMETIC_CROSS_PRODUCT_HPP


#include <Eigen/Core>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>

#include "Utils.h"
#include <vector>

using namespace std;

int main(int argc, char **argv)
{
    cv::Mat cv_mat1 = (cv::Mat_<double>(3, 1) << 1, 2, 3), cv_mat2;
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> eigen_mat;

    cv::cv2eigen(cv_mat1, eigen_mat);
    eigen2cv(eigen_mat, cv_mat2);
    cout << "cv mat: " << cv_mat1.t() << " --> "              //
         << "eigen mat: " << eigen_mat.transpose() << " --> " //
         << "cv mat2: " << cv_mat2.t()                        //
         << endl;

    // MatrixXd m = Eigen::Map<MatrixXd>(reinterpret_cast<double*>(a.data),a.rows,a.cols);

    return 0;
}


#ifndef UTILS_H
#define UTILS_H

#define _USE_MATH_DEFINES
#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <algorithm>

// Clamp a to be within lim1 and lim2
double clamp(double a, double lim1, double lim2);
// Scales input to be between (0 to 1) based on input limits
double scaleFactor(double f, double tl, double tu);
// Converts a rotation matrix to Euler angles (RzRyRx) convention
Eigen::Vector3d Rotation2Euler(const Eigen::Matrix3d& R);
// Computes an iterative inverse kinematics solution
// Eigen::VectorXd IterativeInverseKinematics(std::function<Eigen::VectorXd(const Eigen::VectorXd&)> FK_func, std::function<Eigen::MatrixXd(const Eigen::VectorXd&)> J_FK_func, const Eigen::VectorXd& x_target, const Eigen::VectorXd& theta_init);
// Computes an iterative inverse kinematics solution with holonomic constraints
// Eigen::VectorXd IterativeConstrainedInverseKinematics(std::function<Eigen::VectorXd(const Eigen::VectorXd&)> y_func, std::function<Eigen::MatrixXd(const Eigen::VectorXd&)> J_y_func, std::function<Eigen::VectorXd(const Eigen::VectorXd&)> hol_func, std::function<Eigen::MatrixXd(const Eigen::VectorXd&)> J_hol_func, const Eigen::VectorXd& y_target, const Eigen::VectorXd& q_init);
// Convert from euler angles to rotation matrix using the ZYX convention
Eigen::Matrix3d Euler2Rotation(const Eigen::Vector3d& euler);
// Convert from angular velocity to euler rates using the ZYX convention
Eigen::Vector3d AngularVelocity2EulerRates(const Eigen::Vector3d& euler, const Eigen::Vector3d& w);
// Convert from to euler rates to angular velocity using the ZYX convention
Eigen::Vector3d EulerRates2AngularVelocity(const Eigen::Vector3d& euler, const Eigen::Vector3d& euler_rates);
// Computes skew-symmetric matrix from Vector3d
Eigen::Matrix3d skew(const Eigen::Vector3d& v);

#endif // UTILS_H


/* boost random/beta_distribution.hpp header file
 *
 * Copyright Steven Watanabe 2014
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 */

#ifndef BOOST_RANDOM_BETA_DISTRIBUTION_HPP
#define BOOST_RANDOM_BETA_DISTRIBUTION_HPP

#include <cassert>
#include <istream>
#include <iosfwd>
#include <boost/random/detail/operators.hpp>
#include <boost/random/gamma_distribution.hpp>

namespace boost {
namespace random {

/**
 * The beta distribution is a real-valued distribution which produces
 * values in the range [0, 1].  It has two parameters, alpha and beta.
 *
 * It has \f$\displaystyle p(x) = \frac{x^{\alpha-1}(1-x)^{\beta-1}}{B(\alpha, \beta)}\f$.
 */
template<class RealType = double>
class beta_distribution {
public:
    typedef RealType result_type;
    typedef RealType input_type;

    class param_type {
    public:
        typedef beta_distribution distribution_type;

        /**
         * Constructs a @c param_type from the "alpha" and "beta" parameters
         * of the distribution.
         *
         * Requires: alpha > 0, beta > 0
         */
        explicit param_type(RealType alpha_arg = RealType(1.0),
                            RealType beta_arg = RealType(1.0))
          : _alpha(alpha_arg), _beta(beta_arg)
        {
            assert(alpha_arg > 0);
            assert(beta_arg > 0);
        }

        /** Returns the "alpha" parameter of the distribtuion. */
        RealType alpha() const { return _alpha; }
        /** Returns the "beta" parameter of the distribution. */
        RealType beta() const { return _beta; }

        /** Writes a @c param_type to a @c std::ostream. */
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
        { os << parm._alpha << ' ' << parm._beta; return os; }

        /** Reads a @c param_type from a @c std::istream. */
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
        { is >> parm._alpha >> std::ws >> parm._beta; return is; }

        /** Returns true if the two sets of parameters are the same. */
        BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(param_type, lhs, rhs)
        { return lhs._alpha == rhs._alpha && lhs._beta == rhs._beta; }

        /** Returns true if the two sets of parameters are the different. */
        BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(param_type)

    private:
        RealType _alpha;
        RealType _beta;
    };

    /**
     * Constructs an @c beta_distribution from its "alpha" and "beta" parameters.
     *
     * Requires: alpha > 0, beta > 0
     */
    explicit beta_distribution(RealType alpha_arg = RealType(1.0),
                               RealType beta_arg = RealType(1.0))
      : _alpha(alpha_arg), _beta(beta_arg)
    {
        assert(alpha_arg > 0);
        assert(beta_arg > 0);
    }
    /** Constructs an @c beta_distribution from its parameters. */
    explicit beta_distribution(const param_type& parm)
      : _alpha(parm.alpha()), _beta(parm.beta())
    {}

    /**
     * Returns a random variate distributed according to the
     * beta distribution.
     */
    template<class URNG>
    RealType operator()(URNG& urng) const
    {
        RealType a = gamma_distribution<RealType>(_alpha, RealType(1.0))(urng);
        RealType b = gamma_distribution<RealType>(_beta, RealType(1.0))(urng);
        return a / (a + b);
    }

    /**
     * Returns a random variate distributed accordint to the beta
     * distribution with parameters specified by @c param.
     */
    template<class URNG>
    RealType operator()(URNG& urng, const param_type& parm) const
    {
        return beta_distribution(parm)(urng);
    }

    /** Returns the "alpha" parameter of the distribution. */
    RealType alpha() const { return _alpha; }
    /** Returns the "beta" parameter of the distribution. */
    RealType beta() const { return _beta; }

    /** Returns the smallest value that the distribution can produce. */
    RealType min BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return RealType(0.0); }
    /** Returns the largest value that the distribution can produce. */
    RealType max BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return RealType(1.0); }

    /** Returns the parameters of the distribution. */
    param_type param() const { return param_type(_alpha, _beta); }
    /** Sets the parameters of the distribution. */
    void param(const param_type& parm)
    {
        _alpha = parm.alpha();
        _beta = parm.beta();
    }

    /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
    void reset() { }

    /** Writes an @c beta_distribution to a @c std::ostream. */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, beta_distribution, wd)
    {
        os << wd.param();
        return os;
    }

    /** Reads an @c beta_distribution from a @c std::istream. */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, beta_distribution, wd)
    {
        param_type parm;
        if(is >> parm) {
            wd.param(parm);
        }
        return is;
    }

    /**
     * Returns true if the two instances of @c beta_distribution will
     * return identical sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(beta_distribution, lhs, rhs)
    { return lhs._alpha == rhs._alpha && lhs._beta == rhs._beta; }

    /**
     * Returns true if the two instances of @c beta_distribution will
     * return different sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(beta_distribution)

private:
    RealType _alpha;
    RealType _beta;
};

} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_BETA_DISTRIBUTION_HPP


#include <iostream>
#include <Eigen/Sparse>
#include <Eigen/Cholesky>
#include <Eigen/LU>
#include <cmath>
#include <cassert>
#include <chrono>
#include "writer.hpp"

using namespace std::chrono;
using Real= double;
using SparseMatrix= Eigen::SparseMatrix<Real>;
using Triplet= Eigen::Triplet<Real>;
using Vector= Eigen::VectorXd;

template <typename T>
class Rho
{
    public:
        Rho(const T x_0, const T y_0, const T dx)
         : x_0(x_0), y_0(y_0), dx(dx) {}
        T operator() (const T x, const T y)
        {
           if (!set && abs(x - x_0) < dx / 2 && abs(y - y_0) < dx / 2)
           {
               set= true;
               return (T)1.0 / dx / dx;
           }
           else
           {
               return (T)0.0;
           }
        }
    private:
        const T x_0, y_0, dx;
        bool set{false};
};

void CreatePoissonMatrix(SparseMatrix& A, const int N)
{
    A.resize(N * N, N * N); // resize for unrolling RHS vector
    std::vector<Triplet> triplets; // to build sparse matrix A from
    triplets.reserve(N * N * 5); // each row of A in general has 5 entries - will be somewhat too large
    for (int i= 0; i < N * N; i++)
    {
        triplets.push_back(Triplet(i, i, 4)); // main diagonal
        if (i > 0 && i % N != 0)
            triplets.push_back(Triplet(i, i - 1, -1)); // left off-diagonal: i-1
        if (i < N * N - 1 && (i + 1) % N != 0 || i == 0)
            triplets.push_back(Triplet(i, i + 1, -1)); // right off-diagonal: i+1
        if (i < N * N - N)
            triplets.push_back(Triplet(i, i + N, -1)); // j+1
        if (i > N - 1)
            triplets.push_back(Triplet(i, i - N, -1)); // j-1
    }
    A.setFromTriplets(triplets.begin(), triplets.end());
}

template <class F, typename T>
void CreateRHSVector(Vector& RHS, const int N, const T dx, F& rho)
{
    RHS.resize(N * N); // unroll grid into vector

    auto x= [&dx](T i) {return (i + 1) * dx;};
    auto y= [&dx](T j) {return (j + 1) * dx;};

    const T dx2{dx * dx};

    for (int j= 0; j < N; j++) // row major storage
        for (int i= 0; i < N; i++)
            RHS[j * N + i]= dx2 * rho(x(i), y(j));
}

template <typename T>
Vector SolveJacobi(const SparseMatrix& A, const Vector& RHS, const int N,
                   const T treshold, const int iterations)
{
    Vector x= Vector::Zero((N + 2) * (N + 2));
    Vector x_0= Vector::Ones(N * N);
    Vector x_1;

    int i{0};
    T l2_norm{1.0};

    high_resolution_clock::time_point t_0= high_resolution_clock::now();
    const SparseMatrix D(A.diagonal().asDiagonal());
    const SparseMatrix D_inv(D.diagonal().asDiagonal().inverse());
    const SparseMatrix R= A - D;

    const Vector D_inv_x_RHS= D_inv * RHS; // to speed up calculations during loop
    const SparseMatrix D_inv_x_R= D_inv * R;

    while (l2_norm > treshold && i++ < iterations)
    {
        x_1= D_inv_x_RHS - D_inv_x_R * x_0; // x_1= D_inv * (RHS - R * x_0)
        l2_norm= (x_1 - x_0).norm();
        x_0= x_1;
    }
    high_resolution_clock::time_point t_1= high_resolution_clock::now();
    std::cout << "Jacobi relaxation solver: Success"
              << " after " << i << " iterations in "
              << duration_cast<milliseconds>(t_1 - t_0).count() <<" ms" << std::endl;

    for (int j= 1; j <= N; j++)
        for (int i= 1; i <= N; i++)
                x[j * (N + 2) + i]= x_1[(j - 1) * N + (i - 1)];
    return x;
}

template <typename T>
Vector SolveGaussSeidel(const SparseMatrix& A, const Vector& RHS, const int N,
                        const T treshold, const int iterations)
{
    Vector x= Vector::Zero((N + 2) * (N + 2));
    Vector x_0= Vector::Ones(N * N);
    Vector x_1;

    int i{0};
    T l2_norm{1.0};

    high_resolution_clock::time_point t_0= high_resolution_clock::now();
    const SparseMatrix L= A.triangularView<Eigen::Lower>();
    const SparseMatrix U= A.triangularView<Eigen::StrictlyUpper>();

    Eigen::SparseLU<SparseMatrix> solver; // to get L^{-1}
    solver.compute(L);
    SparseMatrix I(N * N, N * N); I.setIdentity();
    const SparseMatrix L_inv= solver.solve(I);

    const Vector L_inv_x_RHS= L_inv * RHS; // to speed up calculations during loop
    const SparseMatrix L_inv_x_U= L_inv * U;

    while (l2_norm > treshold && i++ < iterations)
    {
        x_1= L_inv_x_RHS - L_inv_x_U * x_0; // x_1= L_inv * (RHS - U * x_0)
        l2_norm= (x_1 - x_0).norm();
        x_0= x_1;
    }
    high_resolution_clock::time_point t_1= high_resolution_clock::now();
    std::cout << "Gauss-Seidel solver: Success"
              << " after " << i << " iterations in "
              << duration_cast<milliseconds>(t_1 - t_0).count() <<" ms" << std::endl;

    for (int j= 1; j <= N; j++)
        for (int i= 1; i <= N; i++)
                x[j * (N + 2) + i]= x_1[(j - 1) * N + (i - 1)];
    return x;
}

template <typename T>
Vector SolveConjugateGradient(const SparseMatrix& A, const Vector& RHS, const int N,
                              const T treshold, const int iterations, const bool use_jacobi_precond=false)
{
    Vector x= Vector::Zero((N + 2) * (N + 2)); // to hold final solution w/ boundary conditions
    Vector x_= Vector::Zero(N * N); // for iterative calculations
    SparseMatrix M_inv= SparseMatrix(N * N, N * N); M_inv.setIdentity(); // no preconditioner
    if (use_jacobi_precond)
        M_inv= A.diagonal().asDiagonal().inverse(); // Jacobi preconditioner
    Vector r_0, r_1; // residuals r
    Vector p, Ap; // "direction vectors" p
    Vector z_0, z_1; // preconditioned residuals z
    T alpha, beta; // Richardson iteration scaling parameters

    int i{0};

    high_resolution_clock::time_point t_0= high_resolution_clock::now();
    r_0= RHS - A * x_;
    z_0= M_inv * r_0;
    p= z_0;

    for ( ; i++ < iterations; )
    {
        Ap= A * p;
        alpha= r_0.dot(z_0) / p.dot(Ap);
        x_= x_ + alpha * p;
        r_1= r_0 - alpha * Ap;
        if (r_1.norm() < treshold) break; // convergence test
        z_1= M_inv * r_1;
        beta= z_1.dot(r_1) / z_0.dot(r_0);
        p= z_1 + beta * p;
        r_0= r_1;
        z_0= z_1;
    }

    high_resolution_clock::time_point t_1= high_resolution_clock::now();
    std::cout << ((use_jacobi_precond == false) ? "Conjugate gradient solver: Success"
                                                : "Conjugate gradient solver w/ Jacobi preconditioner: Success" )
              << " after " << i << " iterations in "
              << duration_cast<milliseconds>(t_1 - t_0).count() <<" ms" << std::endl;

    for (int j= 1; j <= N; j++)
        for (int i= 1; i <= N; i++)
                x[j * (N + 2) + i]= x_[(j - 1) * N + (i - 1)];
    return x;
}

Vector SolveEigenCG(const SparseMatrix& A, const Vector& RHS, const int N)
{
    Vector x= Vector::Zero((N + 2) * (N + 2)); // to fill with solution

    high_resolution_clock::time_point t_0= high_resolution_clock::now();
    Eigen::ConjugateGradient<SparseMatrix, Eigen::Lower|Eigen::Upper> solver;
    solver.compute(A);
    assert (solver.info() == Eigen::Success);
    Vector x_inner= solver.solve(RHS); // solve inner grid
    high_resolution_clock::time_point t_1= high_resolution_clock::now();
    std::cout << "Eigen CG solver: Success in "
              << duration_cast<milliseconds>(t_1 - t_0).count() <<" ms" << std::endl;
    for (int j= 1; j <= N; j++)
        for (int i= 1; i <= N; i++)
                x[j * (N + 2) + i]= x_inner[(j - 1) * N + (i - 1)];
    return x;
}

Vector SolveCholesky(const SparseMatrix& A, const Vector& RHS, const int N)
{
    Vector x= Vector::Zero((N + 2) * (N + 2)); // to fill with solution

    high_resolution_clock::time_point t_0= high_resolution_clock::now();
    Eigen::SparseLU<SparseMatrix> solver;
    solver.compute(A);
    assert (solver.info() == Eigen::Success);
    Vector x_inner= solver.solve(RHS); // solve inner grid
    high_resolution_clock::time_point t_1= high_resolution_clock::now();
    std::cout << "Eigen Cholesky solver: Success in "
              << duration_cast<milliseconds>(t_1 - t_0).count() <<" ms" << std::endl;
    for (int j= 1; j <= N; j++)
        for (int i= 1; i <= N; i++)
                x[j * (N + 2) + i]= x_inner[(j - 1) * N + (i - 1)];
    return x;
}

int main(int argc, char* argv[])
{
     if (argc < 4 || argc > 4 || argv[1] == "-h") // check cl args and give some help
    {
        std::cerr << "Usage: " << argv[0] << "\n\t"
                  << " N(int): grid parameter: number of points per axis" << "\n\t"
                  << " x_0(Real): x coord of point charge in 0..1" << "\n\t"
                  << " y_0(Real): y coord of point charge in 0..1" << "\n\t"
                  << std::endl << std::endl;
        return 1;
    }

    const int N{atoi(argv[1])};
    const Real dx{(Real)1.0 / (Real)(N + 1)};
    const Real x_0{atof(argv[2])}, y_0{atof(argv[3])};

    const Real treshold{0.0001};
    const int iterations{10000};

    SparseMatrix A;
    Vector RHS;
    Vector RHS2;
    Rho rho{x_0, y_0, dx};
    Rho rho2{1.0 - x_0, 1.0 - y_0, dx};
    CreatePoissonMatrix(A, N);
    CreateRHSVector(RHS, N, dx, rho);
    CreateRHSVector(RHS2, N, dx, rho2);
    RHS = (RHS + RHS2) / 2;
    Vector x_cholesky= SolveCholesky(A, RHS, N);
    Vector x_eigen_cg= SolveEigenCG(A, RHS, N);
    // Vector x_jacobi= SolveJacobi(A, RHS, N, treshold, iterations);
    // Vector x_gauss_seidel= SolveGaussSeidel(A, RHS, N, treshold, iterations);
    Vector x_conjugate_gradient= SolveConjugateGradient(A, RHS, N, treshold, iterations);
    Vector x_conjugate_gradient_precon= SolveConjugateGradient(A, RHS, N, treshold, iterations, true);
    // std::cout << "L2 difference Cholesky - Jacobi: " << (x_cholesky - x_jacobi).norm() << std::endl
            //   << "L2 difference Cholseky - Gauss-Seidel: " << (x_cholesky - x_jacobi).norm() << std::endl;
    std::cout << "L2 difference Cholesky - Conjugate Gradient: " << (x_cholesky - x_conjugate_gradient).norm() << std::endl;
    std::cout << "L2 difference Eigen CG - Conjugate Gradient: " << (x_eigen_cg - x_conjugate_gradient).norm() << std::endl;
    std::cout << "L2 difference Eigen CG - Conjugate Gradient Precon: " << (x_eigen_cg - x_conjugate_gradient_precon).norm() << std::endl;
    writeToFile("x_cholesky.txt", x_cholesky);
    writeToFile("x_eigen_cg.txt", x_eigen_cg);
    // writeToFile("x_jacobi.txt", x_jacobi);
    // writeToFile("x_gauss_seidel.txt", x_gauss_seidel);
    writeToFile("x_conjugate_gradient.txt", x_conjugate_gradient);
    writeToFile("x_conjugate_gradient_precon.txt", x_conjugate_gradient_precon);
    // writeToFile("x_diff_chol_jaco.txt", (x_cholesky - x_jacobi));
    // writeToFile("x_diff_chol_gaus.txt", (x_cholesky - x_gauss_seidel));
    writeToFile("x_diff_chol_conj.txt", (x_cholesky - x_conjugate_gradient));
    return 0;
}


#include <boost/numeric/odeint.hpp>
#include <boost/array.hpp>
#include <vector>

using namespace boost::numeric::odeint;

/* The type of container used to hold the state vector */
typedef boost::array<double, 3> state_type;

void lorenz(const state_type &x, state_type &dxdt, const double t)
{
    double sigma = 10.0;
    double R = 28.0;
    double b = 8.0 / 3.0;

    dxdt[0] = sigma * (x[1] - x[0]);
    dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
    dxdt[2] = -b * x[2] + x[0] * x[1];
}

// An example of observer to record steps in the integration
struct boost_array_observer
{
    std::vector<state_type> &m_states;
    std::vector<double> &m_times;

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

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

int main()
{
    // this time, let the num of steps be known
    const auto n_steps = 1000;

    // System state initialization with initial condition
    state_type x = {0.0, 1.0, 0.1};

    // Containers to store solution
    std::vector<state_type> x_sol;
    std::vector<double> times;

    // Set stepper
    runge_kutta4<state_type> rk4;

    // Integrate over time
    auto t_init = 0.0;
    auto t_final = 10.0;
    auto dt = 0.001;
    auto steps = integrate_const(
        rk4,
        lorenz,
        x,
        t_init,
        t_final,
        dt,
        boost_array_observer(x_sol, times));

    // Displaying result on terminal
    for (size_t i = 0; i <= steps; i++)
    {
        std::cout << times[i] << '\t' << x_sol[i][0] << '\t' << x_sol[i][1] << '\t' << x_sol[i][2] << '\n';
    }

    return 0;
}


/*
 * neonGX - Transformations.hpp
 *
 * Copyright (c) 2016, cynecx
 *
 * This file is part of neonGX and is distributed under a modified
 * BSD 3-Clause License. See LICENSE.txt for details.
 *
 */

#ifndef NEONGX_TRANSFORMATIONS_H
#define NEONGX_TRANSFORMATIONS_H

#include <cmath>
#include <neonGX/Core/Math/Primitives.hpp>
#include <neonGX/Core/Math/Matrix.hpp>
#include <boost/math/constants/constants.hpp>

//#define USE_CONSTANT_CACHING

namespace neonGX {

  enum class ConstantCacheType {
    sin,
    cos,
    tan
  };

  float ConstantCache(ConstantCacheType type, float val);

  template <typename T>
  static constexpr T PI = boost::math::constants::pi<T>();

  inline constexpr float DegreeToRadian(float degree) {
    return degree * PI<float> / 180.0f;
  }

  inline constexpr float RadianToDegree(float radian) {
    return radian * 180.0f / PI<float>;
  }

  inline constexpr Matrix3 GetTranslationMatrix(float tx, float ty) {
    return {
        1.0f, 0.0f, tx,
        0.0f, 1.0f, ty,
        0.0f, 0.0f, 1.0f
    };
  }

  inline constexpr Matrix3 GetTranslationMatrix(const FPoint& point) {
    return GetTranslationMatrix(point.x, point.y);
  }

  inline constexpr Matrix3 GetScalingMatrix(float sx, float sy) {
    return {
        sx, 0.0f, 0.0f,
        0.0f, sy, 0.0f,
        0.0f, 0.0f, 1.0f
    };
  }

  inline constexpr Matrix3 GetScalingMatrix(const FPoint& point) {
    return GetScalingMatrix(point.x, point.y);
  }

  inline Matrix3 GetRotationMatrix(float q) {
#ifdef USE_CONSTANT_CACHING
    float qsin = ConstantCache(ConstantCacheType::sin, q);
    float qcos = ConstantCache(ConstantCacheType::cos, q);
#else
    float qsin = sin(q);
    float qcos = cos(q);
#endif

    return {
        qcos, -qsin, 0.0f,
        qsin, qcos, 0.0f,
        0.0f, 0.0f, 1.0f
    };
  }

  inline Matrix3 GetSkewingMatrix(float x, float y) {
#ifdef USE_CONSTANT_CACHING
    float xtan = ConstantCache(ConstantCacheType::tan, x);
    float ytan = ConstantCache(ConstantCacheType::tan, y);
#else
    float xtan = tan(x);
    float ytan = tan(y);
#endif

    return {
        1.0f, xtan, 0.0f,
        ytan, 1.0f, 0.0f,
        0.0f, 0.0f, 1.0f
    };
  }

  inline Matrix3 GetSkewingMatrix(const FPoint& point) {
    return GetSkewingMatrix(point.x, point.y);
  }

  inline constexpr Matrix3 GetIdentityTransform() {
    return {
        1.0f, 0.0f, 0.0f,
        0.0f, 1.0f, 0.0f,
        0.0f, 0.0f, 1.0f
    };
  }

  inline Matrix3 GetToClipspaceTransform(const FSize& destFrame,
                                         const FPoint& sourcePoint,
                                         bool swapHeightSign) {
    // (Position / Resolution) * 2 - 1
    auto toClipspace = Matrix3{
        2.0f / destFrame.width, 0.0f, -1.0f,
        0.0f, (swapHeightSign ? -2.0f : 2.0f) / destFrame.height,
          (swapHeightSign ? 1.0f : -1.0f),
        0.0f, 0.0f, 1.0f
    };

    return toClipspace * GetTranslationMatrix(-sourcePoint.x, -sourcePoint.y);
  }

} // end namespace neonGX

#endif // !NEONGX_TRANSFORMATIONS_H