/********************************************************
  Stanford Driving Software
  Copyright (c) 2011 Stanford University
  All rights reserved.

  Redistribution and use in source and binary forms, with
  or without modification, are permitted provided that the
  following conditions are met:

* Redistributions of source code must retain the above
  copyright notice, this list of conditions and the
  following disclaimer.
* Redistributions in binary form must reproduce the above
  copyright notice, this list of conditions and the
  following disclaimer in the documentation and/or other
  materials provided with the distribution.
* The names of the contributors may not be used to endorse
  or promote products derived from this software
  without specific prior written permission.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
  CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
  INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
  OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
  DAMAGE.
 ********************************************************/

#include <iostream>
#include <Eigen/Dense>
#include <aw_geometry_3d.h>

using namespace Eigen;

namespace vlr {

//== CONSTANTS ========================================================
#define INSIDE  false
#define OUTSIDE true

// two vectors, a and b, starting from c
float dot(const Vector3f& a, const Vector3f& b) {
  return a.dot(b);
}

// two vectors, a and b
Vector3f cross(const Vector3f& a, const Vector3f& b) {
  return a.cross(b);
}

// two vectors, a and b, starting from c
Vector3f cross(const Vector3f& a, const Vector3f& b, const Vector3f& c) {
  float a0 = a(0) - c(0);
  float a1 = a(1) - c(1);
  float a2 = a(2) - c(2);
  float b0 = b(0) - c(0);
  float b1 = b(1) - c(1);
  float b2 = b(2) - c(2);
  return Vector3f(a1 * b2 - a2 * b1, a2 * b0 - a0 * b2, a0 * b1 - a1 * b0);
}

float dist2(const Vector3f& a, const Vector3f& b) {
  float x = a(0) - b(0);
  float y = a(1) - b(1);
  float z = a(2) - b(2);
  return x * x + y * y + z * z;
}

float dist(const Vector3f& a, const Vector3f& b) {
  return sqrtf(dist2(a, b));
}

// linear interpolation
Vector3f lerp(float t, const Vector3f& a, const Vector3f& b) {
  float v[3];
  float u = 1.0 - t;
  v[0] = u * a(0) + t * b(0);
  v[1] = u * a(1) + t * b(1);
  v[2] = u * a(2) + t * b(2);
  return Vector3f(v[0], v[1], v[2]);
}

// is the ball centered at b with radius r
// fully within the box centered at bc, with radius br?
bool ball_within_bounds(const Vector3f& b, float r, const Vector3f& bc, float br) {
  r -= br;
  if ((b(0) - bc(0) <= r) || (bc(0) - b(0) <= r) || (b(1) - bc(1) <= r) || (bc(1) - b(1) <= r) || (b(2) - bc(2) <= r) || (bc(2) - b(2) <= r)) return false;
  return true;
}

// is the ball centered at b with radius r
// fully within the box centered from min to max?
bool ball_within_bounds(const Vector3f& b, float r, const Vector3f& min, const Vector3f& max) {
  if ((b(0) - min(0) <= r) || (max(0) - b(0) <= r) || (b(1) - min(1) <= r) || (max(1) - b(1) <= r) || (b(2) - min(2) <= r) || (max(2) - b(2) <= r)) return false;
  return true;
}

// does the ball centered at b, with radius r,
// intersect the box centered at bc, with radius br?
bool bounds_overlap_ball(const Vector3f& b, float r, const Vector3f& bc, float br) {
  float sum = 0.0, tmp;
  if ((tmp = bc(0) - br - b(0)) > 0.0) {
    if (tmp > r) return false;
    sum += tmp * tmp;
  }
  else if ((tmp = b(0) - (bc(0) + br)) > 0.0) {
    if (tmp > r) return false;
    sum += tmp * tmp;
  }
  if ((tmp = bc(1) - br - b(1)) > 0.0) {
    if (tmp > r) return false;
    sum += tmp * tmp;
  }
  else if ((tmp = b(1) - (bc(1) + br)) > 0.0) {
    if (tmp > r) return false;
    sum += tmp * tmp;
  }
  if ((tmp = bc(2) - br - b(2)) > 0.0) {
    if (tmp > r) return false;
    sum += tmp * tmp;
  }
  else if ((tmp = b(2) - (bc(2) + br)) > 0.0) {
    if (tmp > r) return false;
    sum += tmp * tmp;
  }
  return (sum < r * r);
}

bool bounds_overlap_ball(const Vector3f& b, float r, const Vector3f& min, const Vector3f& max) {
  float sum = 0.0, tmp;
  if (b(0) < min(0)) {
    tmp = min(0) - b(0);
    if (tmp > r) return false;
    sum += tmp * tmp;
  }
  else if (b(0) > max(0)) {
    tmp = b(0) - max(0);
    if (tmp > r) return false;
    sum += tmp * tmp;
  }
  if (b(1) < min(1)) {
    tmp = min(1) - b(1);
    sum += tmp * tmp;
  }
  else if (b(1) > max(1)) {
    tmp = b(1) - max(1);
    sum += tmp * tmp;
  }
  r *= r;
  if (sum > r) return false;
  if (b(2) < min(2)) {
    tmp = min(2) - b(2);
    sum += tmp * tmp;
  }
  else if (b(2) > max(2)) {
    tmp = b(2) - max(2);
    sum += tmp * tmp;
  }
  return (sum < r);
}

// calculate barycentric coordinates of the point p
// (already on the triangle plane) with normal vector n
// and two edge vectors v1 and v2,
// starting from a common vertex t0
void bary_fast(const Vector3f& p, const Vector3f& n, const Vector3f& t0, const Vector3f& v1, const Vector3f& v2, float &b1, float &b2, float &b3) {
  // see bary above
  int i = 0;
  if (n(1) > n(0)) i = 1;
  if (n(2) > n(i)) {
    // ignore z
    float d = 1.0 / (v1(0) * v2(1) - v1(1) * v2(0));
    float x0 = (p(0) - t0(0));
    float x1 = (p(1) - t0(1));
    b1 = (x0 * v2(1) - x1 * v2(0)) * d;
    b2 = (v1(0) * x1 - v1(1) * x0) * d;
  }
  else if (i == 0) {
    // ignore x
    float d = 1.0 / (v1(1) * v2(2) - v1(2) * v2(1));
    float x0 = (p(1) - t0(1));
    float x1 = (p(2) - t0(2));
    b1 = (x0 * v2(2) - x1 * v2(1)) * d;
    b2 = (v1(1) * x1 - v1(2) * x0) * d;
  }
  else {
    // ignore y
    float d = 1.0 / (v1(2) * v2(0) - v1(0) * v2(2));
    float x0 = (p(2) - t0(2));
    float x1 = (p(0) - t0(0));
    b1 = (x0 * v2(0) - x1 * v2(2)) * d;
    b2 = (v1(2) * x1 - v1(0) * x0) * d;
  }
  b3 = 1.0 - b1 - b2;
}

bool closer_on_lineseg(const Vector3f& x, Vector3f& cp, const Vector3f& a, const Vector3f& b, float &d2) {
  Vector3f ba(b(0) - a(0), b(1) - a(1), b(2) - a(2));
  Vector3f xa(x(0) - a(0), x(1) - a(1), x(2) - a(2));

  float xa_ba = dot(xa, ba);
  // if the dot product is negative, the point is closest to a
  if (xa_ba < 0.0) {
    float nd = dist2(x, a);
    if (nd < d2) {
      cp = a;
      d2 = nd;
      return true;
    }
    return false;
  }

  // if the dot product is greater than squared segment length,
  // the point is closest to b
  float fact = xa_ba / ba.squaredNorm();
  if (fact >= 1.0) {
    float nd = dist2(x, b);
    if (nd < d2) {
      cp = b;
      d2 = nd;
      return true;
    }
    return false;
  }

  // take the squared dist x-a, squared dot of x-a to unit b-a,
  // use Pythagoras' rule
  float nd = xa.squaredNorm() - xa_ba * fact;
  if (nd < d2) {
    d2 = nd;
    cp(0) = a(0) + fact * ba(0);
    cp(1) = a(1) + fact * ba(1);
    cp(2) = a(2) + fact * ba(2);
    return true;
  }
  return false;
}

void distance_point_line(const Vector3f& x, const Vector3f& a, const Vector3f& b, float &d2, Vector3f& cp) {
  Vector3f ba(b(0) - a(0), b(1) - a(1), b(2) - a(2));
  Vector3f xa(x(0) - a(0), x(1) - a(1), x(2) - a(2));

  float xa_ba = dot(xa, ba);

  // if the dot product is negative, the point is closest to a
  if (xa_ba < 0.0) {
    d2 = dist2(x, a);
    cp = a;
    return;
  }

  // if the dot product is greater than squared segment length,
  // the point is closest to b
  float fact = xa_ba / ba.squaredNorm();
  if (fact >= 1.0) {
    d2 = dist2(x, b);
    cp = b;
    return;
  }

  // take the squared dist x-a, squared dot of x-a to unit b-a,
  // use Pythagoras' rule
  d2 = xa.squaredNorm() - xa_ba * fact;
  cp(0) = a(0) + fact * ba(0);
  cp(1) = a(1) + fact * ba(1);
  cp(2) = a(2) + fact * ba(2);
  return;
}

void distance_point_tri(const Vector3f& x, const Vector3f& t1, const Vector3f& t2, const Vector3f& t3, float &d2, Vector3f& cp) {
  // calculate the normal and distance from the plane
  Vector3f v1(t2(0) - t1(0), t2(1) - t1(1), t2(2) - t1(2));
  Vector3f v2(t3(0) - t1(0), t3(1) - t1(1), t3(2) - t1(2));
  Vector3f n = cross(v1, v2);
  float n_inv_mag2 = 1.0 / n.squaredNorm();
  float tmp = (x(0) - t1(0)) * n(0) + (x(1) - t1(1)) * n(1) + (x(2) - t1(2)) * n(2);
  float distp2 = tmp * tmp * n_inv_mag2;

  // calculate the barycentric coordinates of the point
  // (projected onto tri plane) with respect to v123
  float b1, b2, b3;
  float f = tmp * n_inv_mag2;
  Vector3f pp(x(0) - f * n(0), x(1) - f * n(1), x(2) - f * n(2));
  bary_fast(pp, n, t1, v1, v2, b1, b2, b3);

  // all non-negative, the point is within the triangle
  if (b1 >= 0.0 && b2 >= 0.0 && b3 >= 0.0) {
    d2 = distp2;
    cp = pp;
    return;
  }

  // look at the signs of the barycentric coordinates
  // if there are two negative signs, the positive
  // one tells the vertex that's closest
  // if there's one negative sign, the opposite edge
  // (with endpoints) is closest

  if (b1 < 0.0) {
    if (b2 < 0.0) {
      d2 = dist2(x, t3);
      cp = t3;
    }
    else if (b3 < 0.0) {
      d2 = dist2(x, t2);
      cp = t2;
    }
    else {
      distance_point_line(x, t2, t3, d2, cp);
    }
  }
  else if (b2 < 0.0) {
    if (b3 < 0.0) {
      d2 = dist2(x, t1);
      cp = t1;
    }
    else {
      distance_point_line(x, t1, t3, d2, cp);
    }
  }
  else {
    distance_point_line(x, t1, t2, d2, cp);
  }
  return;
}

bool closer_on_tri(const Vector3f& x, Vector3f& cp, const Vector3f& t1, const Vector3f& t2, const Vector3f& t3, float &d2) {
  // calculate the normal and distance from the plane
  Vector3f v1(t2(0) - t1(0), t2(1) - t1(1), t2(2) - t1(2));
  Vector3f v2(t3(0) - t1(0), t3(1) - t1(1), t3(2) - t1(2));
  Vector3f n = cross(v1, v2);
  float n_inv_mag2 = 1.0 / n.squaredNorm();
  float tmp = (x(0) - t1(0)) * n(0) + (x(1) - t1(1)) * n(1) + (x(2) - t1(2)) * n(2);
  float distp2 = tmp * tmp * n_inv_mag2;
  if (distp2 >= d2) return false;

  // calculate the barycentric coordinates of the point
  // (projected onto tri plane) with respect to v123
  float b1, b2, b3;
  float f = tmp * n_inv_mag2;
  Vector3f pp(x(0) - f * n(0), x(1) - f * n(1), x(2) - f * n(2));
  bary_fast(pp, n, t1, v1, v2, b1, b2, b3);

  // all non-negative, the point is within the triangle
  if (b1 >= 0.0 && b2 >= 0.0 && b3 >= 0.0) {
    d2 = distp2;
    cp = pp;
    return true;
  }

  // look at the signs of the barycentric coordinates
  // if there are two negative signs, the positive
  // one tells the vertex that's closest
  // if there's one negative sign, the opposite edge
  // (with endpoints) is closest

  if (b1 < 0.0) {
    if (b2 < 0.0) {
      float nd = dist2(x, t3);
      if (nd < d2) {
        d2 = nd;
        cp = t3;
        return true;
      }
      else {
        return false;
      }
    }
    else if (b3 < 0.0) {
      float nd = dist2(x, t2);
      if (nd < d2) {
        d2 = nd;
        cp = t2;
        return true;
      }
      else {
        return false;
      }
    }
    else return closer_on_lineseg(x, cp, t2, t3, d2);
  }
  else if (b2 < 0.0) {
    if (b3 < 0.0) {
      float nd = dist2(x, t1);
      if (nd < d2) {
        d2 = nd;
        cp = t1;
        return true;
      }
      else {
        return false;
      }
    }
    else return closer_on_lineseg(x, cp, t1, t3, d2);
  }
  else return closer_on_lineseg(x, cp, t1, t2, d2);
}

// calculate the intersection of a line going through p
// to direction dir with a plane spanned by t1,t2,t3
// (modified from Graphics Gems, p.299)
bool line_plane_X(const Vector3f& p, const Vector3f& dir, const Vector3f& t1, const Vector3f& t2, const Vector3f& t3, Vector3f& x, float &dist) {
  // note: normal doesn't need to be unit vector
  Vector3f nrm = cross(t1, t2, t3);
  float tmp = dot(nrm, dir);
  if (tmp == 0.0) {
    std::cerr << "Cannot intersect plane with a parallel line" << std::endl;
    return false;
  }
  // d  = -dot(nrm,t1)
  // t  = - (d + dot(p,nrm))/dot(dir,nrm)
  // is = p + dir * t
  x = dir;
  dist = (dot(nrm, t1) - dot(nrm, p)) / tmp;
  x *= dist;
  x += p;
  if (dist < 0.0) dist = -dist;
  return true;
}

bool line_plane_X(const Vector3f& p, const Vector3f& dir, const Vector3f& nrm, float d, Vector3f& x, float &dist) {
  float tmp = dot(nrm, dir);
  if (tmp == 0.0) {
    std::cerr << "Cannot intersect plane with a parallel line" << std::endl;
    return false;
  }
  x = dir;
  dist = -(d + dot(nrm, p)) / tmp;
  x *= dist;
  x += p;
  if (dist < 0.0) dist = -dist;
  return true;
}

// calculate barycentric coordinates of the point p
// on triangle t1 t2 t3
void bary(const Vector3f& p, const Vector3f& t1, const Vector3f& t2, const Vector3f& t3, float &b1, float &b2, float &b3) {
  // figure out the plane onto which to project the vertices
  // by calculating a cross product and finding its largest dimension
  // then use Cramer's rule to calculate two of the
  // barycentric coordinates
  // e.g., if the z coordinate is ignored, and v1 = t1-t3, v2 = t2-t3
  // b1 = det(gx(0)g v2(0); x(1) v2(1)) / det(v1(0) v2(0); v1(1) v2(1))
  // b2 = det(gv1(0)g x(0); v1(1) x(1)) / det(v1(0) v2(0); v1(1) v2(1))
  float v10 = t1(0) - t3(0);
  float v11 = t1(1) - t3(1);
  float v12 = t1(2) - t3(2);
  float v20 = t2(0) - t3(0);
  float v21 = t2(1) - t3(1);
  float v22 = t2(2) - t3(2);
  float c[2];
  c[0] = fabs(v11 * v22 - v12 * v21);
  c[1] = fabs(v12 * v20 - v10 * v22);
  int i = 0;
  if (c[1] > c[0]) i = 1;
  if (fabs(v10 * v21 - v11 * v20) > c[i]) {
    // ignore z
    float d = 1.0f / (v10 * v21 - v11 * v20);
    float x0 = (p(0) - t3(0));
    float x1 = (p(1) - t3(1));
    b1 = (x0 * v21 - x1 * v20) * d;
    b2 = (v10 * x1 - v11 * x0) * d;
  }
  else if (i == 0) {
    // ignore x
    float d = 1.0f / (v11 * v22 - v12 * v21);
    float x0 = (p(1) - t3(1));
    float x1 = (p(2) - t3(2));
    b1 = (x0 * v22 - x1 * v21) * d;
    b2 = (v11 * x1 - v12 * x0) * d;
  }
  else {
    // ignore y
    float d = 1.0f / (v12 * v20 - v10 * v22);
    float x0 = (p(2) - t3(2));
    float x1 = (p(0) - t3(0));
    b1 = (x0 * v20 - x1 * v22) * d;
    b2 = (v12 * x1 - v10 * x0) * d;
  }
  b3 = 1.0f - b1 - b2;
}

// calculate barycentric coordinates for the intersection of
// a line starting from p, going to direction dir, and the plane
// of the triangle t1 t2 t3
bool bary(const Vector3f& p, const Vector3f& dir, const Vector3f& t1, const Vector3f& t2, const Vector3f& t3, float &b1, float &b2, float &b3) {
  Vector3f x;
  float d;
  if (!line_plane_X(p, dir, t1, t2, t3, x, d)) return false;
  bary(x, t1, t2, t3, b1, b2, b3);

  return true;
}

// calculate the intersection of a line starting from p,
// going to direction dir, and the triangle t1 t2 t3
bool line_tri_X(const Vector3f& p, const Vector3f& dir, const Vector3f& t1, const Vector3f& t2, const Vector3f& t3, Vector3f& x, float& d) {
  float b1, b2, b3;
  Vector3f x_temp;
  float d_temp;
  if (!line_plane_X(p, dir, t1, t2, t3, x_temp, d_temp)) return false;

  bary(x_temp, t1, t2, t3, b1, b2, b3);
  // all non-negative, the point is within the triangle
  if (b1 >= 0.0 && b2 >= 0.0 && b3 >= 0.0) {
    x = x_temp;
    d = d_temp;
    return true;
  }
  return false;
}

} // namespace vlr


/* chi_squared_test.hpp header file
 *
 * Copyright Steven Watanabe 2010
 * 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)
 *
 * $Id$
 *
 */

#ifndef BOOST_RANDOM_TEST_CHI_SQUARED_TEST_HPP_INCLUDED
#define BOOST_RANDOM_TEST_CHI_SQUARED_TEST_HPP_INCLUDED

#include <vector>

#include <boost/math/special_functions/pow.hpp>
#include <boost/math/distributions/chi_squared.hpp>

// This only works for discrete distributions with fixed
// upper and lower bounds.

template<class IntType>
struct chi_squared_collector {

    static const IntType cutoff = 5;

    chi_squared_collector()
      : chi_squared(0),
        variables(0),
        prev_actual(0),
        prev_expected(0),
        current_actual(0),
        current_expected(0)
    {}

    void operator()(IntType actual, double expected) {
        current_actual += actual;
        current_expected += expected;

        if(current_expected >= cutoff) {
            if(prev_expected != 0) {
                update(prev_actual, prev_expected);
            }
            prev_actual = current_actual;
            prev_expected = current_expected;

            current_actual = 0;
            current_expected = 0;
        }
    }

    void update(IntType actual, double expected) {
        chi_squared += boost::math::pow<2>(actual - expected) / expected;
        ++variables;
    }

    double cdf() {
        if(prev_expected != 0) {
            update(prev_actual + current_actual, prev_expected + current_expected);
            prev_actual = 0;
            prev_expected = 0;
            current_actual = 0;
            current_expected = 0;
        }
        if(variables <= 1) {
            return 0;
        } else {
            return boost::math::cdf(boost::math::chi_squared(variables - 1), chi_squared);
        }
    }

    double chi_squared;
    std::size_t variables;

    IntType prev_actual;
    double prev_expected;

    IntType current_actual;
    double current_expected;
};

template<class IntType>
double chi_squared_test(const std::vector<IntType>& results, const std::vector<double>& probabilities, IntType iterations) {
    chi_squared_collector<IntType> calc;
    for(std::size_t i = 0; i < results.size(); ++i) {
        calc(results[i], iterations * probabilities[i]);
    }
    return calc.cdf();
}

#endif


// Copyright John Maddock 2012.
// 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)

#ifndef BOOST_MATH_AIRY_HPP
#define BOOST_MATH_AIRY_HPP

#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/cbrt.hpp>

namespace boost{ namespace math{

namespace detail{

template <class T, class Policy>
T airy_ai_imp(T x, const Policy& pol)
{
   BOOST_MATH_STD_USING

   if(x < 0)
   {
      T p = (-x * sqrt(-x) * 2) / 3;
      T v = T(1) / 3;
      T j1 = boost::math::cyl_bessel_j(v, p, pol);
      T j2 = boost::math::cyl_bessel_j(-v, p, pol);
      T ai = sqrt(-x) * (j1 + j2) / 3;
      //T bi = sqrt(-x / 3) * (j2 - j1);
      return ai;
   }
   else if(fabs(x * x * x) / 6 < tools::epsilon<T>())
   {
      T tg = boost::math::tgamma(constants::twothirds<T>(), pol);
      T ai = 1 / (pow(T(3), constants::twothirds<T>()) * tg);
      //T bi = 1 / (sqrt(boost::math::cbrt(T(3))) * tg);
      return ai;
   }
   else
   {
      T p = 2 * x * sqrt(x) / 3;
      T v = T(1) / 3;
      //T j1 = boost::math::cyl_bessel_i(-v, p, pol);
      //T j2 = boost::math::cyl_bessel_i(v, p, pol);
      //
      // Note that although we can calculate ai from j1 and j2, the accuracy is horrible
      // as we're subtracting two very large values, so use the Bessel K relation instead:
      //
      T ai = cyl_bessel_k(v, p, pol) * sqrt(x / 3) / boost::math::constants::pi<T>();  //sqrt(x) * (j1 - j2) / 3;
      //T bi = sqrt(x / 3) * (j1 + j2);
      return ai;
   }
}

template <class T, class Policy>
T airy_bi_imp(T x, const Policy& pol)
{
   BOOST_MATH_STD_USING

   if(x < 0)
   {
      T p = (-x * sqrt(-x) * 2) / 3;
      T v = T(1) / 3;
      T j1 = boost::math::cyl_bessel_j(v, p, pol);
      T j2 = boost::math::cyl_bessel_j(-v, p, pol);
      //T ai = sqrt(-x) * (j1 + j2) / 3;
      T bi = sqrt(-x / 3) * (j2 - j1);
      return bi;
   }
   else if(fabs(x * x * x) / 6 < tools::epsilon<T>())
   {
      T tg = boost::math::tgamma(constants::twothirds<T>(), pol);
      //T ai = 1 / (pow(T(3), constants::twothirds<T>()) * tg);
      T bi = 1 / (sqrt(boost::math::cbrt(T(3))) * tg);
      return bi;
   }
   else
   {
      T p = 2 * x * sqrt(x) / 3;
      T v = T(1) / 3;
      T j1 = boost::math::cyl_bessel_i(-v, p, pol);
      T j2 = boost::math::cyl_bessel_i(v, p, pol);
      T bi = sqrt(x / 3) * (j1 + j2);
      return bi;
   }
}

template <class T, class Policy>
T airy_ai_prime_imp(T x, const Policy& pol)
{
   BOOST_MATH_STD_USING

   if(x < 0)
   {
      T p = (-x * sqrt(-x) * 2) / 3;
      T v = T(2) / 3;
      T j1 = boost::math::cyl_bessel_j(v, p, pol);
      T j2 = boost::math::cyl_bessel_j(-v, p, pol);
      T aip = -x * (j1 - j2) / 3;
      return aip;
   }
   else if(fabs(x * x) / 2 < tools::epsilon<T>())
   {
      T tg = boost::math::tgamma(constants::third<T>(), pol);
      T aip = 1 / (boost::math::cbrt(T(3)) * tg);
      return -aip;
   }
   else
   {
      T p = 2 * x * sqrt(x) / 3;
      T v = T(2) / 3;
      //T j1 = boost::math::cyl_bessel_i(-v, p, pol);
      //T j2 = boost::math::cyl_bessel_i(v, p, pol);
      //
      // Note that although we can calculate ai from j1 and j2, the accuracy is horrible
      // as we're subtracting two very large values, so use the Bessel K relation instead:
      //
      T aip = -cyl_bessel_k(v, p, pol) * x / (boost::math::constants::root_three<T>() * boost::math::constants::pi<T>());
      return aip;
   }
}

template <class T, class Policy>
T airy_bi_prime_imp(T x, const Policy& pol)
{
   BOOST_MATH_STD_USING

   if(x < 0)
   {
      T p = (-x * sqrt(-x) * 2) / 3;
      T v = T(2) / 3;
      T j1 = boost::math::cyl_bessel_j(v, p, pol);
      T j2 = boost::math::cyl_bessel_j(-v, p, pol);
      T aip = -x * (j1 + j2) / constants::root_three<T>();
      return aip;
   }
   else if(fabs(x * x) / 2 < tools::epsilon<T>())
   {
      T tg = boost::math::tgamma(constants::third<T>(), pol);
      T bip = sqrt(boost::math::cbrt(T(3))) / tg;
      return bip;
   }
   else
   {
      T p = 2 * x * sqrt(x) / 3;
      T v = T(2) / 3;
      T j1 = boost::math::cyl_bessel_i(-v, p, pol);
      T j2 = boost::math::cyl_bessel_i(v, p, pol);
      T aip = x * (j1 + j2) / boost::math::constants::root_three<T>();
      return aip;
   }
}

} // namespace detail

template <class T, class Policy>
inline typename tools::promote_args<T>::type airy_ai(T x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, Policy>(detail::airy_ai_imp<value_type>(static_cast<value_type>(x), forwarding_policy()), "boost::math::airy<%1%>(%1%)");
}

template <class T>
inline typename tools::promote_args<T>::type airy_ai(T x)
{
   return airy_ai(x, policies::policy<>());
}

template <class T, class Policy>
inline typename tools::promote_args<T>::type airy_bi(T x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, Policy>(detail::airy_bi_imp<value_type>(static_cast<value_type>(x), forwarding_policy()), "boost::math::airy<%1%>(%1%)");
}

template <class T>
inline typename tools::promote_args<T>::type airy_bi(T x)
{
   return airy_bi(x, policies::policy<>());
}

template <class T, class Policy>
inline typename tools::promote_args<T>::type airy_ai_prime(T x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, Policy>(detail::airy_ai_prime_imp<value_type>(static_cast<value_type>(x), forwarding_policy()), "boost::math::airy<%1%>(%1%)");
}

template <class T>
inline typename tools::promote_args<T>::type airy_ai_prime(T x)
{
   return airy_ai_prime(x, policies::policy<>());
}

template <class T, class Policy>
inline typename tools::promote_args<T>::type airy_bi_prime(T x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, Policy>(detail::airy_bi_prime_imp<value_type>(static_cast<value_type>(x), forwarding_policy()), "boost::math::airy<%1%>(%1%)");
}

template <class T>
inline typename tools::promote_args<T>::type airy_bi_prime(T x)
{
   return airy_bi_prime(x, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_AIRY_HPP


/**
 * \file SinusGeneratorFilter.cpp
 */

#include "SimpleSinusGeneratorFilter.h"

#include <boost/math/constants/constants.hpp>

#include <cmath>
#include <cstdint>

namespace ATK
{
  template<class DataType_>
  SimpleSinusGeneratorFilter<DataType_>::SimpleSinusGeneratorFilter()
  :TypedBaseFilter<DataType_>(0, 1)
  {
  }

  template<class DataType_>
  void SimpleSinusGeneratorFilter<DataType_>::set_amplitude(DataType_ amplitude)
  {
    this->amplitude = amplitude;
  }

  template<class DataType_>
  void SimpleSinusGeneratorFilter<DataType_>::set_frequency(int frequency)
  {
    this->frequency = frequency;
  }

  template<class DataType_>
  void SimpleSinusGeneratorFilter<DataType_>::process_impl(gsl::index size) const
  {
    double real_increment = 2. / output_sampling_rate * frequency;

    for(gsl::index i = 0; i < size; ++i)
    {
      state += real_increment;
      outputs[0][i] = static_cast<DataType_>(amplitude * std::sin(state * boost::math::constants::pi<double>()));
    }
  }

  template class SimpleSinusGeneratorFilter<std::int16_t>;
  template class SimpleSinusGeneratorFilter<std::int32_t>;
  template class SimpleSinusGeneratorFilter<int64_t>;
  template class SimpleSinusGeneratorFilter<float>;
  template class SimpleSinusGeneratorFilter<double>;
}


/*
 * NativeCommonOps.cpp
 *
 *  Created on: Nov 27, 2018
 *      Author: Georg Wiedebach
 */

#include <jni.h>
#include <Eigen/Dense>
#include <iostream>
#include "us_ihmc_matrixlib_NativeCommonOpsWrapper.h"

using Eigen::MatrixXd;

JNIEXPORT void JNICALL Java_us_ihmc_matrixlib_NativeCommonOpsWrapper_mult(JNIEnv *env, jobject thisObj,
		jdoubleArray result, jdoubleArray aData, jdoubleArray bData, jint aRows, jint aCols, jint bCols)
{
	jdouble *aDataArray = env->GetDoubleArrayElements(aData, NULL);
	jdouble *bDataArray = env->GetDoubleArrayElements(bData, NULL);
	MatrixXd A = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(aDataArray, aRows, aCols);
	MatrixXd B = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(bDataArray, aCols, bCols);

	MatrixXd AB = A * B;

	jdouble *resultDataArray = new jdouble[aRows * bCols];
	Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(resultDataArray, aRows, bCols) = AB;
	env->SetDoubleArrayRegion(result, 0, aRows * bCols, resultDataArray);

	env->ReleaseDoubleArrayElements(aData, aDataArray, 0);
	env->ReleaseDoubleArrayElements(bData, bDataArray, 0);
	delete resultDataArray;
}

JNIEXPORT void JNICALL Java_us_ihmc_matrixlib_NativeCommonOpsWrapper_multQuad(JNIEnv *env, jobject thisObj,
		jdoubleArray result, jdoubleArray aData, jdoubleArray bData, jint aRows, jint aCols)
{
	jdouble *aDataArray = env->GetDoubleArrayElements(aData, NULL);
	jdouble *bDataArray = env->GetDoubleArrayElements(bData, NULL);
	MatrixXd A = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(aDataArray, aRows, aCols);
	MatrixXd B = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(bDataArray, aRows, aRows);

	MatrixXd AtBA = A.transpose() * B * A;

	jdouble *resultDataArray = new jdouble[aCols * aCols];
	Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(resultDataArray, aCols, aCols) = AtBA;
	env->SetDoubleArrayRegion(result, 0, aCols * aCols, resultDataArray);

	env->ReleaseDoubleArrayElements(aData, aDataArray, 0);
	env->ReleaseDoubleArrayElements(bData, bDataArray, 0);
	delete resultDataArray;
}

JNIEXPORT void JNICALL Java_us_ihmc_matrixlib_NativeCommonOpsWrapper_invert(JNIEnv *env, jobject thisObj,
		jdoubleArray result, jdoubleArray aData, jint aRows)
{
	jdouble *aDataArray = env->GetDoubleArrayElements(aData, NULL);
	MatrixXd A = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(aDataArray, aRows, aRows);

	MatrixXd x = A.lu().inverse();

	jdouble *resultDataArray = new jdouble[aRows * aRows];
	Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(resultDataArray, aRows, aRows) = x;
	env->SetDoubleArrayRegion(result, 0, aRows * aRows, resultDataArray);

	env->ReleaseDoubleArrayElements(aData, aDataArray, 0);
	delete resultDataArray;
}

JNIEXPORT void JNICALL Java_us_ihmc_matrixlib_NativeCommonOpsWrapper_solve(JNIEnv *env, jobject thisObj,
		jdoubleArray result, jdoubleArray aData, jdoubleArray bData, jint aRows)
{
	jdouble *aDataArray = env->GetDoubleArrayElements(aData, NULL);
	jdouble *bDataArray = env->GetDoubleArrayElements(bData, NULL);
	MatrixXd A = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(aDataArray, aRows, aRows);
	MatrixXd B = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(bDataArray, aRows, 1);

	MatrixXd x = A.lu().solve(B);

	jdouble *resultDataArray = new jdouble[aRows];
	Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(resultDataArray, aRows, 1) = x;
	env->SetDoubleArrayRegion(result, 0, aRows, resultDataArray);

	env->ReleaseDoubleArrayElements(aData, aDataArray, 0);
	env->ReleaseDoubleArrayElements(bData, bDataArray, 0);
}

JNIEXPORT jboolean JNICALL Java_us_ihmc_matrixlib_NativeCommonOpsWrapper_solveCheck(JNIEnv *env, jobject thisObj,
		jdoubleArray result, jdoubleArray aData, jdoubleArray bData, jint aRows)
{
	jdouble *aDataArray = env->GetDoubleArrayElements(aData, NULL);
	jdouble *bDataArray = env->GetDoubleArrayElements(bData, NULL);
	MatrixXd A = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(aDataArray, aRows, aRows);
	MatrixXd B = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(bDataArray, aRows, 1);

	const Eigen::FullPivLU<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> > fullPivLu = A.fullPivLu();
	if (fullPivLu.isInvertible())
	{
		MatrixXd x = fullPivLu.solve(B);

		jdouble *resultDataArray = new jdouble[aRows];
		Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(resultDataArray, aRows, 1) = x;
		env->SetDoubleArrayRegion(result, 0, aRows, resultDataArray);

		delete resultDataArray;
		env->ReleaseDoubleArrayElements(aData, aDataArray, 0);
		env->ReleaseDoubleArrayElements(bData, bDataArray, 0);
		return true;
	}
	else
	{
		env->ReleaseDoubleArrayElements(aData, aDataArray, 0);
		env->ReleaseDoubleArrayElements(bData, bDataArray, 0);
		return false;
	}
}

JNIEXPORT void JNICALL Java_us_ihmc_matrixlib_NativeCommonOpsWrapper_solveRobust(JNIEnv *env, jobject thisObj,
		jdoubleArray result, jdoubleArray aData, jdoubleArray bData, jint aRows, jint aCols)
{
	jdouble *aDataArray = env->GetDoubleArrayElements(aData, NULL);
	jdouble *bDataArray = env->GetDoubleArrayElements(bData, NULL);
	MatrixXd A = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(aDataArray, aRows, aCols);
	MatrixXd B = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(bDataArray, aRows, 1);

	MatrixXd x = A.householderQr().solve(B);

	jdouble *resultDataArray = new jdouble[aCols];
	Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(resultDataArray, aCols, 1) = x;
	env->SetDoubleArrayRegion(result, 0, aCols, resultDataArray);

	env->ReleaseDoubleArrayElements(aData, aDataArray, 0);
	env->ReleaseDoubleArrayElements(bData, bDataArray, 0);
	delete resultDataArray;
}

JNIEXPORT void JNICALL Java_us_ihmc_matrixlib_NativeCommonOpsWrapper_solveDamped(JNIEnv *env, jobject thisObj,
		jdoubleArray result, jdoubleArray aData, jdoubleArray bData, jint aRows, jint aCols, jdouble alpha)
{
	jdouble *aDataArray = env->GetDoubleArrayElements(aData, NULL);
	jdouble *bDataArray = env->GetDoubleArrayElements(bData, NULL);
	MatrixXd A = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(aDataArray, aRows, aCols);
	MatrixXd B = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(bDataArray, aRows, 1);

	MatrixXd outer = A * A.transpose() + MatrixXd::Identity(aRows, aRows) * alpha * alpha;
	MatrixXd x = A.transpose() * outer.llt().solve(B);

	jdouble *resultDataArray = new jdouble[aCols];
	Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(resultDataArray, aCols, 1) = x;
	env->SetDoubleArrayRegion(result, 0, aCols, resultDataArray);

	env->ReleaseDoubleArrayElements(aData, aDataArray, 0);
	env->ReleaseDoubleArrayElements(bData, bDataArray, 0);
	delete resultDataArray;
}

JNIEXPORT void JNICALL Java_us_ihmc_matrixlib_NativeCommonOpsWrapper_projectOnNullspace(JNIEnv *env, jobject thisObj,
		jdoubleArray result, jdoubleArray aData, jdoubleArray bData, jint aRows, jint aCols, jint bRows, jdouble alpha)
{
	jdouble *aDataArray = env->GetDoubleArrayElements(aData, NULL);
	jdouble *bDataArray = env->GetDoubleArrayElements(bData, NULL);
	MatrixXd A = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(aDataArray, aRows, aCols);
	MatrixXd B = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(bDataArray, bRows, aCols);

	MatrixXd BtB = B.transpose() * B;
	MatrixXd outer = BtB + MatrixXd::Identity(aCols, aCols) * alpha * alpha;
	MatrixXd x = A * (MatrixXd::Identity(aCols, aCols) - outer.llt().solve(BtB));

	jdouble *resultDataArray = new jdouble[aRows * aCols];
	Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(resultDataArray, aRows, aCols) = x;
	env->SetDoubleArrayRegion(result, 0, aRows * aCols, resultDataArray);

	env->ReleaseDoubleArrayElements(aData, aDataArray, 0);
	env->ReleaseDoubleArrayElements(bData, bDataArray, 0);
	delete resultDataArray;
}


//------------------------------------------------------------------------------
/// \file Radix_tests.cpp
/// \author Ernest Yeung
/// \email  ernestyalumni@gmail.com
/// \ref https://en.wikipedia.org/wiki/Radix
/// https://www.youtube.com/watch?v=NLKQEOgBAnw
/// Algorithms: Bit Manipulation. HackerRank. Gayle Laakmann McDowell.
/// \details To run only the Bits unit tests, do this:
/// ./Check --run_test="Algorithms/Bits"
///
/// Also, consider running the BooleanAlgebra_tests, as such:
/// ./Check --run_test="Utilities/BooleanAlgebra_tests"
//------------------------------------------------------------------------------
#include "Algorithms/Bits/Masks.h"
#include "Algorithms/Bits/Shift.h"
#include "Cpp/Numerics/BitCast.h"
#include "Cpp/Utilities/SuperBitSet.h"
#include "Utilities/EndianConversions.h"
#include "Utilities/ToHexString.h"

#include <boost/test/unit_test.hpp>
#include <cstdint>
#include <iostream>
#include <limits>

using Algorithms::Bits::clear_bit;
using Algorithms::Bits::is_bit_set_high;
using Algorithms::Bits::set_bit_high;
using Algorithms::Bits::logical_right_shift;
using Cpp::Numerics::bit_cast;
using Cpp::Utilities::SuperBitSet;
using Cpp::Utilities::number_of_bits_in_a_byte;
using Utilities::ToHexString;
using Utilities::to_big_endian;
using Utilities::to_little_endian;

BOOST_AUTO_TEST_SUITE(Algorithms)
BOOST_AUTO_TEST_SUITE(Bits)
BOOST_AUTO_TEST_SUITE(NumericalRepresentation_tests)

// cf. https://www.cs.utexas.edu/users/fussell/courses/cs429h/lectures/Lecture_2-429h.pdf
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(BinaryRepresentation)
{
  // Limits
  {
    std::cout << "\n\n Unsigned limits \n";

    std::cout << "std::numeric_limits<uint8_t>::max(): " <<
      static_cast<unsigned int>(std::numeric_limits<uint8_t>::max()) << "\n";
        // 255
    std::cout << "std::numeric_limits<uint16_t>::max(): " <<
      std::numeric_limits<uint16_t>::max() << "\n"; // 65535
    std::cout << "std::numeric_limits<uint32_t>::max(): " <<
      std::numeric_limits<uint32_t>::max() << "\n"; // 4294967295
    std::cout << "std::numeric_limits<uint64_t>::max(): " <<
      std::numeric_limits<uint64_t>::max() << "\n"; // 18446744073709551615

    std::cout << "\n FloatingPoint limits \n";

    std::cout << "std::numeric_limits<float>::max(): " <<
      std::numeric_limits<float>::max() << "\n";
    std::cout << "std::numeric_limits<double>::max(): " <<
      std::numeric_limits<double>::max() << "\n";
  }

  {
    std::cout << "\n\n Signed limits \n";

    std::cout << "std::numeric_limits<int8_t>::max(): " <<
      static_cast<unsigned int>(std::numeric_limits<int8_t>::max()) << "\n";
        // 127
    std::cout << "std::numeric_limits<int16_t>::max(): " <<
      std::numeric_limits<int16_t>::max() << "\n"; // 32767
    std::cout << "std::numeric_limits<int32_t>::max(): " <<
      std::numeric_limits<int32_t>::max() << "\n"; // 2147483647
    std::cout << "std::numeric_limits<int64_t>::max(): " <<
      std::numeric_limits<int64_t>::max() << "\n"; // 18446744073709551615

    std::cout << "std::numeric_limits<int8_t>::min(): " <<
      static_cast<int>(std::numeric_limits<int8_t>::min()) << "\n";
        // -128
    std::cout << "std::numeric_limits<int16_t>::min(): " <<
      std::numeric_limits<int16_t>::min() << "\n"; // -32768
    std::cout << "std::numeric_limits<int32_t>::min(): " <<
      std::numeric_limits<int32_t>::min() << "\n"; // -2147483648
    std::cout << "std::numeric_limits<int64_t>::min(): " <<
      std::numeric_limits<int64_t>::min() << "\n"; // 18446744073709551615
  }

  // Examples
  {
    std::cout << "\n Examples of Binary Representations \n";

    {
      SuperBitSet<sizeof(uint16_t) * number_of_bits_in_a_byte> x {15213};
      std::cout << "15213_10 as binary: " << x.to_string() << "\n";
    }
  }

}

// cf. https://en.cppreference.com/w/cpp/language/integer_literal
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(IntegerLiteralBitsAsChars)
{
  {
    const uint8_t au {0b00};
    BOOST_TEST_REQUIRE(au == 0);

    SuperBitSet<8> bits8 {au};
    BOOST_TEST(bits8.to_string() == "00000000");

    const char ac {0b00};
    SuperBitSet<8> cbits8 {ac};
    BOOST_TEST(cbits8.to_string() == "00000000");
  }
  {
    const uint8_t au {0b01};
    BOOST_TEST_REQUIRE(au == 1);

    SuperBitSet<8> bits8 {au};
    BOOST_TEST(bits8.to_string() == "00000001");

    const char ac {0b01};
    SuperBitSet<8> cbits8 {ac};
    BOOST_TEST(cbits8.to_string() == "00000001");
  }
  {
    const uint8_t au {0b10};
    BOOST_TEST_REQUIRE(au == 2);

    SuperBitSet<8> bits8 {au};
    BOOST_TEST(bits8.to_string() == "00000010");

    const char ac {0b10};
    SuperBitSet<8> cbits8 {ac};
    BOOST_TEST(cbits8.to_string() == "00000010");
  }
  {
    const uint8_t au {0b11};
    BOOST_TEST_REQUIRE(au == 3);

    SuperBitSet<8> bits8 {au};
    BOOST_TEST(bits8.to_string() == "00000011");

    const char ac {0b11};
    SuperBitSet<8> cbits8 {ac};
    BOOST_TEST(cbits8.to_string() == "00000011");
  }
}

// cf. https://www.youtube.com/watch?v=NLKQEOgBAnw
// Algorithms: Bit Manipulation,  HackerRank with Gayle Laakmann McDowell.
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(BinaryAdditionCarriesOne)
{
  const uint8_t a {0b0101};
  const uint8_t b {0b0011};
  const uint8_t c {a + b};
  SuperBitSet<8> c8 {c};
  BOOST_TEST(c8.to_string() == "00001000");
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(TwosComplement)
{
  // cf. https://www.cs.utexas.edu/users/fussell/courses/cs429h/lectures/Lecture_3-429h.pdf
  // "Encoding Integers."
  {
    const int16_t x {15213};
    const SuperBitSet<16> xbits16 {x};
    BOOST_TEST(xbits16.to_string() == "0011101101101101");
    ToHexString<int16_t> xh {x};

    // *** stack smashing detected ***: terminated
    // unknown location(0): fatal error: in "Algorithms/Bits/NumericalRepresentation_tests/TwosComplement": signal: SIGABRT (application abort requested)
    //BOOST_TEST(xh() == "6d3b");
    ToHexString<int16_t> be_xh {to_big_endian(xh)};
    //BOOST_TEST(be_xh() == "3b6d");

    const int16_t y {-15213};
    const auto yu = bit_cast<uint16_t>(y);
    const SuperBitSet<16> yubits16 {yu};
    BOOST_TEST(yubits16.to_string() == "1100010010010011");
    ToHexString<int16_t> yh {y};
    //BOOST_TEST(yh() == "93c4");
    ToHexString<int16_t> be_yh {to_big_endian(yh)};
    //BOOST_TEST(be_yh() == "c493");
  }

  {
    const int8_t a {18};
    const auto au = bit_cast<uint8_t>(a);
    SuperBitSet<8> aubits8 {au};
    // Sign bit is 0 for positive.
    BOOST_TEST(aubits8.to_string() == "00010010");
  }
  {
    const int8_t a {-18};
    const auto au = bit_cast<uint8_t>(a);
    SuperBitSet<8> aubits8 {au};
    // Sign bit is 0 for positive.
    BOOST_TEST(aubits8.to_string() == "11101110");
  }
}

// cf. http://sandbox.mc.edu/~bennet/cs110/tc/add.html
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(
  TwosComplementBinaryAdditionWhenSumIsNotArithemticallyCorrect)
{
  // Overflow, no carryout (at sign bit). Sum is not correct.
  {
    const int8_t x {104};
    const SuperBitSet<8> xbits8 {x};
    BOOST_TEST(xbits8.to_string() == "01101000");
    ToHexString<int8_t> xh {x};
    //BOOST_TEST(xh() == "68");

    const int8_t y {45};
    const SuperBitSet<8> ybits8 {y};
    BOOST_TEST(ybits8.to_string() == "00101101");
    ToHexString<int8_t> yh {y};
    //BOOST_TEST(yh() == "2d");

    const auto z = x + y;
    BOOST_TEST(sizeof(z) == 4);
    const SuperBitSet<8> zbits8 {z};
    BOOST_TEST(zbits8.to_string() == "10010101");
    BOOST_TEST(zbits8.to_ulong() == 149);

    // "Wraps around" to -128 and "up".
    const int8_t z8 {-107};
    const SuperBitSet<8> z8bits8 {bit_cast<uint8_t>(z8)};
    BOOST_TEST(z8bits8.to_string() == "10010101");
  }
  // Overflow, with incidental carryout. Sum is not correct.
  {
    const int8_t x {-103};
    const SuperBitSet<8> xbits8 {bit_cast<uint8_t>(x)};
    BOOST_TEST(xbits8.to_string() == "10011001");
    ToHexString<int8_t> xh {x};

    // *** stack smashing detected ***: terminated
    // unknown location(0): fatal error: in "Algorithms/Bits/NumericalRepresentation_tests/TwosComplementBinaryAdditionWhenSumIsNotArithemticallyCorrect": signal: SIGABRT (application abort requested)
    //BOOST_TEST(xh() == "99");

    const int8_t y {-69};
    const SuperBitSet<8> ybits8 {bit_cast<uint8_t>(y)};
    BOOST_TEST(ybits8.to_string() == "10111011");
    ToHexString<int8_t> yh {y};

    // *** stack smashing detected ***: terminated
    // unknown location(0): fatal error: in "Algorithms/Bits/NumericalRepresentation_tests/TwosComplementBinaryAdditionWhenSumIsNotArithemticallyCorrect": signal: SIGABRT (application abort requested)
    //BOOST_TEST(yh() == "bb");

    const auto z = x + y;
    BOOST_TEST(sizeof(z) == 4);
    const SuperBitSet<32> zbits32 {bit_cast<uint32_t>(z)};
    BOOST_TEST(zbits32.to_string() == "11111111111111111111111101010100");
    BOOST_TEST(z == -172);

    // "Wraps around" to -128 and "up".
    const int8_t z8 {84};
    const SuperBitSet<8> z8bits8 {bit_cast<uint8_t>(z8)};
    BOOST_TEST(z8bits8.to_string() == "01010100");
  }
}

// cf. https://www.youtube.com/watch?v=NLKQEOgBAnw
// Algorithms: Bit Manipulation,  HackerRank with Gayle Laakmann McDowell.
// This test breaks down step by step the algorithm Gayle described clearly that
// also explains the name two's complement to get the additive inverse (i.e.
// the negative) of a positive integer.
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(GetAdditiveInverseFromComplement)
{
  const int8_t x {18};
  SuperBitSet<8> xbits8 {bit_cast<uint8_t>(x)};
  BOOST_TEST(xbits8.to_string() == "00010010");

  BOOST_TEST((~xbits8).to_string() == "11101101");

  const int8_t not_x {~x};
  SuperBitSet<8> not_xbits8 {bit_cast<uint8_t>(not_x)};
  BOOST_TEST(not_xbits8.to_string() == "11101101");

  const int8_t not_x_plus_1 {not_x + 1};
  SuperBitSet<8> not_x_plus_1bits8 {bit_cast<uint8_t>(not_x_plus_1)};
  BOOST_TEST(not_x_plus_1bits8.to_string() == "11101110");

  const int8_t y {-18};
  SuperBitSet<8> ybits8 {bit_cast<uint8_t>(y)};
  BOOST_TEST(ybits8.to_string() == "11101110");
  BOOST_TEST(ybits8.to_string() == not_x_plus_1bits8.to_string());
}

// cf. https://youtu.be/NLKQEOgBAnw?t=399
// Logical right shift fills in zero for sign bit.
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(LogicalRightShiftOnNegativeNumbers)
{
  SuperBitSet<8> xbits8 {bit_cast<uint8_t>(logical_right_shift<int8_t>(23, 1))};
  BOOST_TEST(xbits8.to_string() == "00001011");

  int8_t y {-23};
  SuperBitSet<8> ybits8 {bit_cast<uint8_t>(y)};
  BOOST_TEST(ybits8.to_string() == "11101001");

  SuperBitSet<8> zbits8 {bit_cast<uint8_t>(logical_right_shift<int8_t>(y, 1))};
  BOOST_TEST(zbits8.to_string() == "01110100");
}

// cf. https://youtu.be/NLKQEOgBAnw?t=399
// Arithmetic right shift shifts everything to the right including sign bit,
// fills in sign bit with original sign bit.
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ArithmeticRightShiftOnNegativeNumbers)
{
  int8_t x {-23};
  int8_t y {1};
  // Works, but it warns. Narrowing conversion.
  //int8_t z {x >> y};
  //SuperBitSet<8> xbits8 {bit_cast<uint8_t>(z)};
  //BOOST_TEST(xbits8.to_string() == "11110100");
  {
    int8_t x {-22};
    SuperBitSet<8> xbits8 {bit_cast<uint8_t>(x)};
    BOOST_TEST(xbits8.to_string() == "11101010");
    // Works but it warns. Narrowing conversion.
    //int8_t y {x >> 1};
    //SuperBitSet<8> ybits8 {bit_cast<uint8_t>(y)};
    //BOOST_TEST(ybits8.to_string() == "11110101");
  }
}

// cf. https://en.cppreference.com/w/cpp/language/operator_arithmetic
// lhs << rhs
// left shift of lhs by rhs bits.
// lhs >> rhs
// right shift of lhs by rhs bits
// For built-in operators, lhs and rhs must both have integral or unscoped
// enumeration type. Integral promotions performed on both operands.
// Return type is type of left operand after integral promotions.
// For unsigned a, value of a << b is value a * 2^b, reduced modulo 2^N, where
// N is number of bits in return type (that is, bitwise left shift is performed
// and bits that get shifted out of destination type are discarded).
// For negative a, behavior of a << b is undefined.
//
// since C++14
// For signed and non-negative a, if a*2^b representable in unsigned version
// of return type, then taht value, converted to signed, is value of a << b
// For negative a, value of a >> b is implementation-defined (in most
// implementations, this performs arithmetic right shift, so result remains
// negative)
//
// since C++20
// a << b is unique value congrent to a * 2^b modulo 2^N where N is number of
// bits in return type (that is bitwise left shift performed and bits that get
// shifted out of destination type are discarded).
// Value of a >> b is a/2^b, rounded down (i.e., right shift on signed a is
// arithmetic right shift).

BOOST_AUTO_TEST_SUITE(BitwiseShiftOperator_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(Examples)
{
  char c {0x10};
  SuperBitSet<8> cbits8 {bit_cast<uint8_t>(c)};
  BOOST_TEST(cbits8.to_string() == "00010000");

  SuperBitSet<32> xbits32 {bit_cast<uint32_t>(c << 10)};
  BOOST_TEST(
    xbits32.to_string() == "00000000000000000100000000000000");
  BOOST_TEST((c << 10) == 0x4000);

  unsigned long long ull {0x123};
  SuperBitSet<number_of_bits_in_a_byte * sizeof(unsigned long long)>
    ullbits_ull {bit_cast<uint64_t>(ull)};
  BOOST_TEST(
    ullbits_ull.to_string() ==
      "0000000000000000000000000000000000000000000000000000000100100011");

  {
    SuperBitSet<number_of_bits_in_a_byte * sizeof(unsigned long long)>
      xbits_ull {bit_cast<uint64_t>(ull << 1)};

    BOOST_TEST(
      xbits_ull.to_string() ==
        "0000000000000000000000000000000000000000000000000000001001000110");

    BOOST_TEST(xbits_ull.to_ullong() == 0x246);
  }
  {
    // overflow in unsigned
    SuperBitSet<number_of_bits_in_a_byte * sizeof(unsigned long long)>
      xbits_ull {bit_cast<uint64_t>(ull << 63)};

    BOOST_TEST(
      xbits_ull.to_string() ==
        "1000000000000000000000000000000000000000000000000000000000000000");

    BOOST_TEST(xbits_ull.to_ullong() == 0x8000000000000000);
  }

  // For negative a, value of a >> b in most implementations is arithmetic right
  // shift, shifts everything to right including sign bit, fills in sign bit
  // with original sign bit.
  long long ll {-1000};
  SuperBitSet<number_of_bits_in_a_byte * sizeof(long long)>
    llbits_ll {bit_cast<uint64_t>(ll)};
  BOOST_TEST(
    llbits_ll.to_string() ==
      "1111111111111111111111111111111111111111111111111111110000011000");

  SuperBitSet<number_of_bits_in_a_byte * sizeof(long long)>
    xbits_ll {bit_cast<uint64_t>(ll >> 1)};
  BOOST_TEST(
    xbits_ll.to_string() ==
      "1111111111111111111111111111111111111111111111111111111000001100");
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(BitwiseLeftShiftOfNegativeNumbers)
{
  int8_t x {-5};
  SuperBitSet<8> xbits8 {bit_cast<uint8_t>(x)};
  // Indeed this is the "two's complement" of 00000101 = 5; take the complement
  // and then add 1.
  BOOST_TEST(xbits8.to_string() == "11111011");

  // cf. https://en.cppreference.com/w/cpp/language/operator_arithmetic
  // cppreference says this is undefined.
  x << 1;

  auto y {x << 1};

  // std::cout << y << "\n"; // -10

  // template argument deduction/substitution failed.
  // SuperBitSet<8> ybits8 {bit_cast<uint8_t>(x << 1)};
  // SuperBitSet<8> ybits8 {bit_cast<int8_t>(x << 1)};
  //SuperBitSet<16> ybits16 {bit_cast<uint16_t>(x << 1)};
  //SuperBitSet<16> ybits16 {bit_cast<int16_t>(x << 1)};

  SuperBitSet<32> ybits32 {bit_cast<uint32_t>(y)};

  BOOST_TEST(ybits32.to_string() == "11111111111111111111111111110110");
}

BOOST_AUTO_TEST_SUITE_END() // BitwiseShiftOperator_tests

BOOST_AUTO_TEST_SUITE(Masks_tests)

// https://youtu.be/NLKQEOgBAnw?t=444
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(BitwiseAndToGetIthBit)
{
  int8_t x {44};

  SuperBitSet<8> xbits8 {bit_cast<uint8_t>(x)};
  BOOST_TEST(xbits8.to_string() == "00101100");

  // (x & (1 << c)) != 0
  BOOST_TEST((x & (1 << 5)) != 0);

  BOOST_TEST((x & (1 << 7)) == 0);
  BOOST_TEST((x & (1 << 6)) == 0);
  BOOST_TEST((x & (1 << 4)) == 0);
  BOOST_TEST((x & (1 << 1)) == 0);
  BOOST_TEST((x & (1 << 0)) == 0);

  BOOST_TEST((x & (1 << 3)) != 0);
  BOOST_TEST((x & (1 << 2)) != 0);

  BOOST_TEST(!is_bit_set_high(x, 0));
  BOOST_TEST(!is_bit_set_high(x, 1));
  BOOST_TEST(!is_bit_set_high(x, 4));
  BOOST_TEST(!is_bit_set_high(x, 6));
  BOOST_TEST(!is_bit_set_high(x, 7));

  BOOST_TEST(is_bit_set_high(x, 2));
  BOOST_TEST(is_bit_set_high(x, 3));
  BOOST_TEST(is_bit_set_high(x, 5));

  int8_t y {-44}; // 11010100

  BOOST_TEST(is_bit_set_high(y, 2));
  BOOST_TEST(is_bit_set_high(y, 4));
  BOOST_TEST(is_bit_set_high(y, 6));
  BOOST_TEST(is_bit_set_high(y, 7));

  BOOST_TEST(!is_bit_set_high(y, 0));
  BOOST_TEST(!is_bit_set_high(y, 1));
  BOOST_TEST(!is_bit_set_high(y, 3));
  BOOST_TEST(!is_bit_set_high(y, 5));
}

// https://youtu.be/NLKQEOgBAnw?t=466
// Set ith bit, x | (1 << i)
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(BitwiseOrToSetIthBit)
{
  int8_t x {44};

  BOOST_TEST(is_bit_set_high(x, 5));
  x = set_bit_high(x, 5);
  BOOST_TEST(is_bit_set_high(x, 5));

  BOOST_TEST(!is_bit_set_high(x, 6));
  x = set_bit_high(x, 6);
  BOOST_TEST(is_bit_set_high(x, 6));
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ClearBitWithMaskWithAll1ButThatSpot)
{
  int8_t x {44};
  BOOST_TEST(is_bit_set_high(x, 5));
  x = clear_bit(x, 5);
  BOOST_TEST(!is_bit_set_high(x, 5));
}

BOOST_AUTO_TEST_SUITE_END() // Masks_tests

BOOST_AUTO_TEST_SUITE_END() // Radix_tests
BOOST_AUTO_TEST_SUITE_END() // Bits
BOOST_AUTO_TEST_SUITE_END() // Algorithms


// SPDX-License-Identifier: BSD-2-Clause

#ifndef ROS_UTILS_HPP
#define ROS_UTILS_HPP

#include <Eigen/Dense>

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/TransformStamped.h>

namespace hdl_graph_slam {

/**
 * @brief convert Eigen::Matrix to geometry_msgs::TransformStamped
 * @param stamp            timestamp
 * @param pose             Eigen::Matrix to be converted
 * @param frame_id         tf frame_id
 * @param child_frame_id   tf child frame_id
 * @return converted TransformStamped
 */
static geometry_msgs::TransformStamped matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id) {
  Eigen::Quaternionf quat(pose.block<3, 3>(0, 0));
  quat.normalize();
  geometry_msgs::Quaternion odom_quat;
  odom_quat.w = quat.w();
  odom_quat.x = quat.x();
  odom_quat.y = quat.y();
  odom_quat.z = quat.z();

  geometry_msgs::TransformStamped odom_trans;
  odom_trans.header.stamp = stamp;
  odom_trans.header.frame_id = frame_id;
  odom_trans.child_frame_id = child_frame_id;

  odom_trans.transform.translation.x = pose(0, 3);
  odom_trans.transform.translation.y = pose(1, 3);
  odom_trans.transform.translation.z = pose(2, 3);
  odom_trans.transform.rotation = odom_quat;

  return odom_trans;
}

static Eigen::Isometry3d pose2isometry(const geometry_msgs::Pose& pose) {
  Eigen::Isometry3d mat = Eigen::Isometry3d::Identity();
  mat.translation() = Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z);
  mat.linear() = Eigen::Quaterniond(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z).toRotationMatrix();
  return mat;
}

static Eigen::Isometry3d tf2isometry(const tf::StampedTransform& trans) {
  Eigen::Isometry3d mat = Eigen::Isometry3d::Identity();
  mat.translation() = Eigen::Vector3d(trans.getOrigin().x(), trans.getOrigin().y(), trans.getOrigin().z());
  mat.linear() = Eigen::Quaterniond(trans.getRotation().w(), trans.getRotation().x(), trans.getRotation().y(), trans.getRotation().z()).toRotationMatrix();
  return mat;
}

static geometry_msgs::Pose isometry2pose(const Eigen::Isometry3d& mat) {
  Eigen::Quaterniond quat(mat.linear());
  Eigen::Vector3d trans = mat.translation();

  geometry_msgs::Pose pose;
  pose.position.x = trans.x();
  pose.position.y = trans.y();
  pose.position.z = trans.z();
  pose.orientation.w = quat.w();
  pose.orientation.x = quat.x();
  pose.orientation.y = quat.y();
  pose.orientation.z = quat.z();

  return pose;
}

static Eigen::Isometry3d odom2isometry(const nav_msgs::OdometryConstPtr& odom_msg) {
  const auto& orientation = odom_msg->pose.pose.orientation;
  const auto& position = odom_msg->pose.pose.position;

  Eigen::Quaterniond quat;
  quat.w() = orientation.w;
  quat.x() = orientation.x;
  quat.y() = orientation.y;
  quat.z() = orientation.z;

  Eigen::Isometry3d isometry = Eigen::Isometry3d::Identity();
  isometry.linear() = quat.toRotationMatrix();
  isometry.translation() = Eigen::Vector3d(position.x, position.y, position.z);
  return isometry;
}

static Eigen::Isometry2d odom2isometry2d(const nav_msgs::OdometryConstPtr& odom_msg) {
  const auto& orientation = odom_msg->pose.pose.orientation;
  const auto& position = odom_msg->pose.pose.position;

  Eigen::Quaterniond quat;
  quat.w() = orientation.w;
  quat.x() = orientation.x;
  quat.y() = orientation.y;
  quat.z() = orientation.z;

  Eigen::Vector3d ea = (quat.toRotationMatrix()).eulerAngles(2, 1, 0);
  //std::cout << "to ypr angles: " << ea << std::endl;
  //std::cout << "yawss: " << ea[0] << std::endl;

  Eigen::Isometry2d isometry = Eigen::Isometry2d::Identity();
  Eigen::Rotation2D<double> rot(ea[0]);
  isometry.linear() = rot.toRotationMatrix();
  //std::cout << "rot1: " << cos(ea[0]) << " " << -sin(ea[0]) << " " << sin(ea[0]) << " " << cos(ea[0]) << std::endl;
  //std::cout << "rot2: " << isometry.linear() << std::endl;
  isometry.translation() = Eigen::Vector2d(position.x, position.y);
  //std::cout << "tr: " << isometry.translation() << std::endl;
  //std::cout << "iso: " << isometry.matrix() << std::endl;
  return isometry;
}

}  // namespace hdl_graph_slam

#endif  // ROS_UTILS_HPP


#include <cstdlib>
#include <iostream>
#include <complex>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <Eigen/Core>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/eigen/vector.hpp>
#include <boost/numeric/bindings/eigen/matrix.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include "print.hpp"
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

int main(int argc, char *argv[]) {
  {
    typedef std::complex<double> complex;
    typedef ublas::vector<complex> vector;
    typedef ublas::matrix<complex, ublas::column_major> matrix;
    typedef vector::size_type size_type;
    rand_normal<complex>::reset();
    size_type m=6, n=8;
    matrix A(m, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<m; ++i)
 	A(i, j)=rand_normal<complex>::get();
    matrix A_t(ublas::trans(A));
    vector x(n);
    for (size_type i=0; i<n; ++i)
      x(i)=rand_normal<complex>::get();
    vector y(m);
    for (size_type i=0; i<m; ++i)
      y(i)=rand_normal<complex>::get();
    complex alpha(rand_normal<complex>::get());
    complex beta(rand_normal<complex>::get());
    vector y1(alpha*ublas::prod(A, x)+beta*y);
    vector y2(y);
    blas::gemv(alpha, A, x, beta, y2);
    vector y3(y);
    blas::gemv(alpha, blas::trans(A_t), x, beta, y3);
    std::cout << "testing boost::ublas containers\n"
	      << "using ublas           : " << print_vec(y1) << '\n'
	      << "using blas            : " << print_vec(y2) << '\n'
	      << "using blas (tranposed): " << print_vec(y3) << '\n'
	      << '\n';
  }
  {
    typedef std::complex<double> complex;
    typedef Eigen::Matrix<complex, Eigen::Dynamic, 1> vector;
    typedef Eigen::Matrix<complex, Eigen::Dynamic, Eigen::Dynamic> matrix;
    typedef int size_type;
    size_type m=6, n=8;
    rand_normal<complex>::reset();
    matrix A(m, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<m; ++i)
  	A(i, j)=rand_normal<complex>::get();
    matrix A_t(A.transpose());
    vector x(n);
    for (size_type i=0; i<n; ++i)
      x(i)=rand_normal<complex>::get();
    vector y(m);
    for (size_type i=0; i<m; ++i)
      y(i)=rand_normal<complex>::get();
    complex alpha(rand_normal<complex>::get());
    complex beta(rand_normal<complex>::get());
    vector y1(alpha*A*x+beta*y);
    vector y2(y);
    blas::gemv(alpha, A, x, beta, y2);
    vector y3(y);
    blas::gemv(alpha, blas::trans(A_t), x, beta, y3);
    std::cout << "testing eigen++ containers\n"
	      << "using eigen++         : " << print_vec(y1) << '\n'
	      << "using blas            : " << print_vec(y2) << '\n'
	      << "using blas (tranposed): " << print_vec(y3) << '\n'
	      << '\n';
  }
  return EXIT_SUCCESS;
}


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

/*
 Copyright (C) 2015 Johannes Goettker-Schnetmann
 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.
*/

#include <ql/experimental/finitedifferences/squarerootprocessrndcalculator.hpp>

#include <boost/math/distributions/non_central_chi_squared.hpp>

namespace QuantLib {

    SquareRootProcessRNDCalculator::SquareRootProcessRNDCalculator(
        Real v0, Real kappa, Real theta, Real sigma)
    : v0_(v0), kappa_(kappa), theta_(theta),
      d_(4*kappa/(sigma*sigma)), df_(d_*theta) {    }

    Real SquareRootProcessRNDCalculator::pdf(Real v, Time t) const {
        const Real e   = std::exp(-kappa_*t);
        const Real k   = d_/(1-e);
        const Real ncp = k*v0_*e;

        const boost::math::non_central_chi_squared_distribution<Real>
            dist(df_, ncp);

        return boost::math::pdf(dist, v*k) * k;
    }

    Real SquareRootProcessRNDCalculator::cdf(Real v, Time t) const {
        const Real e   = std::exp(-kappa_*t);
        const Real k   = d_/(1-e);
        const Real ncp = k*v0_*e;

        const boost::math::non_central_chi_squared_distribution<Real>
            dist(df_, ncp);

        return boost::math::cdf(dist, v*k);
    }

    Real SquareRootProcessRNDCalculator::invcdf(Real q, Time t) const {
        const Real e   = std::exp(-kappa_*t);
        const Real k   = d_/(1-e);
        const Real ncp = k*v0_*e;

        const boost::math::non_central_chi_squared_distribution<Real>
            dist(df_, ncp);

        return boost::math::quantile(dist, q) / k;
    }

    Real SquareRootProcessRNDCalculator::stationary_pdf(Real v) const {
        const Real alpha = 0.5*df_;
        const Real beta = alpha/theta_;

        return std::pow(beta, alpha)*std::pow(v, alpha-1)
                *std::exp(-beta*v-boost::math::lgamma(alpha));
    }

    Real SquareRootProcessRNDCalculator::stationary_cdf(Real v) const {
        const Real alpha = 0.5*df_;
        const Real beta = alpha/theta_;

        return boost::math::gamma_p(alpha, beta*v);
    }

    Real SquareRootProcessRNDCalculator::stationary_invcdf(Real q) const {
        const Real alpha = 0.5*df_;
        const Real beta = alpha/theta_;

        return boost::math::gamma_p_inv(alpha, q)/beta;
    }
}


#ifndef _DEPTH_ESTIMATOR_H_
#define _DEPTH_ESTIMATOR_H_

#include <vector>
#include <Eigen/Dense>
#include <memory>
#include <ceres/ceres.h>

namespace depth_estimator {

  class CostFunction {
  public:
    explicit inline CostFunction(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2,
                                 int idx1, int idx2,
                                 double target_dist)
      : x1_(p1[0]), y1_(p1[1]),
        x2_(p2[0]), y2_(p2[1]),
        idx1_(idx1), idx2_(idx2),
        dist_(target_dist) {}

    template <typename T>
    inline bool operator() (const T* const d, T* residual) const {
      residual[0] = T(dist_) - ceres::sqrt(ceres::pow(d[idx1_] * T(x1_) - d[idx2_] * T(x2_), 2) +
                                           ceres::pow(d[idx1_] * T(y1_) - d[idx2_] * T(y2_), 2) +
                                           ceres::pow(d[idx1_]          - d[idx2_],          2));
      return true;
    }

  private:
    const double x1_;
    const double y1_;
    const double x2_;
    const double y2_;
    const int idx1_;
    const int idx2_;
    const double dist_;
  };

  template <int N>
  class DepthEstimator {
  public:
    inline DepthEstimator(const std::vector<Eigen::Vector2d>& points_2d,
                          const std::vector<Eigen::Vector3d>& points_3d)
      : points_2d_(points_2d), points_3d_(points_3d) {}

    inline Eigen::VectorXd Estimate() {
      std::vector<double> depths(N, 1.0);

      ceres::Problem problem;

      for (int i = N - 1; i >= 1; --i) {
        for (int j = i - 1; j >= 0; --j) {
          problem.AddResidualBlock(
            new ceres::AutoDiffCostFunction<CostFunction, 1, N>(
              new CostFunction(points_2d_[i], points_2d_[j],
                               i, j,
                               (points_3d_[i] - points_3d_[j]).norm())),
            nullptr, depths.data());
        }
      }

      ceres::Solver::Options options;
      options.linear_solver_type = ceres::DENSE_QR;
      options.minimizer_progress_to_stdout = false;
      options.parameter_tolerance = 0.01;

      ceres::Solver::Summary summary;
      ceres::Solve(options, &problem, &summary);

      Eigen::VectorXd result(N);
      for (int i = 0; i < N; ++i) {
        result[i] = depths[i];
      }
      return result;
    }

  private:
    const std::vector<Eigen::Vector2d>& points_2d_;
    const std::vector<Eigen::Vector3d>& points_3d_;
  };

}

#endif


/* ----------------------------------------------------------------------------
 * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 * @file  testDynamics.cpp
 * @brief Test calculations for statics.
 * @author Frank Dellaert
 */

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>

#include <boost/bind.hpp>
#include <cmath>

#include "gtdynamics/dynamics/Dynamics.h"
#include "gtdynamics/universal_robot/RobotModels.h"

using namespace gtdynamics;
using namespace gtsam;

namespace example {
constexpr double g = 9.8;
const Robot robot = gtdynamics::CreateRobotFromFile(
    kSdfPath + std::string("/test/four_bar_linkage.sdf"));
Vector3 gravity(0, 0, -g);
}  // namespace example

TEST(Dynamics, Coriolis) {
  using namespace example;
  auto inertia = robot.link("l1")->inertiaMatrix();
  Matrix6 actualH;
  auto twist = (Vector(6) << 1, 2, 3, 4, 5, 6).finished();
  const Vector6 expected =
      gtsam::Pose3::adjointTranspose(twist, inertia * twist);
  EXPECT(assert_equal(expected, Coriolis(inertia, twist, actualH), 1e-6));
  Matrix6 numericalH = numericalDerivative11<Vector6, Vector6>(
      boost::bind(&Coriolis, inertia, _1, boost::none), twist);
  EXPECT(assert_equal(numericalH, actualH, 1e-6));
}

int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix3i m = Matrix3i::Random();
cout << "Here is the matrix m:" << endl << m << endl;
cout << "Here are the coefficients on the main diagonal of m:" << endl
     << m.diagonal() << endl;

  return 0;
}


//          Copyright Alain Miniussi 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)

// Authors: Alain Miniussi

#include <vector>
#include <iostream>

#include <boost/mpi/communicator.hpp>
#include <boost/mpi/collectives.hpp>
#include <boost/mpi/environment.hpp>
#include <boost/mpi/cartesian_communicator.hpp>

#include <boost/test/minimal.hpp>

namespace mpi = boost::mpi;
// Curly brace init make this useless, but
//  - Need to support obsolete like g++ 4.3.x. for some reason
//  - Can't conditionnaly compile with bjam (unless you find
//  the doc, and read it, which would only make sense if you
//  actually wan't to use bjam, which does not (make sense))
typedef mpi::cartesian_dimension cd;

int test_main(int argc, char* argv[])
{
  mpi::environment  env;
  mpi::communicator world;

  if (world.size() != 24)  return -1;
  mpi::cartesian_dimension dims[] = {cd(2, true), cd(3,true), cd(4,true)};
  mpi::cartesian_communicator cart(world, mpi::cartesian_topology(dims));
  for (int r = 0; r < cart.size(); ++r) {
    cart.barrier();
    if (r == cart.rank()) {
      std::vector<int> c = cart.coordinates(r);
      std::cout << "rk :" << r << " coords: "
                << c[0] << ' ' << c[1] << ' ' << c[2] << '\n';
    }
  }
  return 0;
}


// unit test file sinc.hpp for the special functions test suite

//  (C) Copyright Hubert Holin 2003. Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.

#include <functional>
#include <iomanip>
#include <iostream>
#include <complex>

#include <boost/math/special_functions/sinc.hpp>

#include <boost/test/unit_test.hpp>

template<typename T>
void    sinc_pi_test(const char * more_blurb)
{
    using    ::std::abs;

    using    ::std::numeric_limits;

    using    ::boost::math::sinc_pi;

    BOOST_MESSAGE("Testing sinc_pi in the real domain for "
        << more_blurb << ".");

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(), 2,
        (
            abs(sinc_pi<T>(static_cast<T>(0))-static_cast<T>(1)),
            numeric_limits<T>::epsilon()
        ));
}

template<typename T>
void    sinc_pi_complex_test(const char * more_blurb)
{
    using    ::std::abs;
    using    ::std::sinh;

    using    ::std::numeric_limits;

    using    ::boost::math::sinc_pi;

    BOOST_MESSAGE("Testing sinc_pi in the complex domain for "
        << more_blurb << ".");

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(), 2,
        (
            abs(sinc_pi<T>(::std::complex<T>(0, 1))-
                ::std::complex<T>(sinh(static_cast<T>(1)))),
            numeric_limits<T>::epsilon()
        ));
}

void    sinc_pi_manual_check()
{
    using    ::boost::math::sinc_pi;

    BOOST_MESSAGE("sinc_pi");

    for    (int i = 0; i <= 100; i++)
    {
        BOOST_MESSAGE( ::std::setw(15)
                    << sinc_pi<float>(static_cast<float>(i-50)/
                                                static_cast<float>(50))
                    << ::std::setw(15)
                    << sinc_pi<double>(static_cast<double>(i-50)/
                                                static_cast<double>(50))
                    << ::std::setw(15)
                    << sinc_pi<long double>(static_cast<long double>(i-50)/
                                                static_cast<long double>(50)));
    }

    BOOST_MESSAGE(" ");
}


#ifndef USE_CUDA

#include <iostream>

#include <kernel.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

// C++ Version
namespace Kernel
{
    double dot(const std::vector<Eigen::Vector3d> & v1, const std::vector<Eigen::Vector3d> & v2)
    {
        double x=0;
        for (int i=0; i<v1.size(); ++i)
        {
            x += v1[i].dot(v2[i]);
        }
        return x;
    }

    void run_eigen_solver(const std::vector<Eigen::Matrix3f> &m)
    {
        for (int i = 0; i < m.size(); ++i){
            Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> es(m[i]);
            std::cout << "Matrix " << i << ":" << std::endl << m[i] << std::endl;
            std::cout << "The eigenvalues :" << std::endl << es.eigenvalues() << std::endl;
            std::cout << "The eigenvectors :" << std::endl << es.eigenvectors() << std::endl;
            std::cout << "==================================================================" << std::endl;
        }
    }
}

#endif


#include <boost/lexical_cast.hpp>
#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <string>

#include "quadrature/qmaxwell.hpp"

using namespace std;
using namespace boltzmann;

// template<typename MAP>
// void print(const MAP& m, std::ofstream& fout, string title)
// {
//   fout << "----- " << title << endl;
//   for (auto it = m.begin(); it != m.end(); ++it) {
//     fout << it->first.first << "\t"
//          << it->first.second
//          << "\t"
//          << setprecision(16) << it->second
//          << endl;
//   }
// }

// template<typename CONT>
// void print_basis(const CONT& cont, std::ofstream& fout)
// {
//   for (int i = 0; i < cont.size(); ++i) {
//     fout  << cont[i].get_id() << endl;
//   }
// }

// ----------------------------------------------------------------------
int main(int argc, char *argv[])
{
  const double beta = 2;

  if (argc < 2) {
    cerr << "info: " << argv[0] << " N" << endl << "q: No. quad. points\n";
    return 1;
  }
  int N = atoi(argv[1]);

  int digits = 128;
  QMaxwell qmaxwell(1, N, digits);

  std::ofstream fout("quadrule_order" + boost::lexical_cast<string>(N) + "_" +
                     boost::lexical_cast<string>(digits) + ".dat");
  for (int i = 0; i < qmaxwell.size(); ++i) {
    fout << setprecision(30) << qmaxwell.pts(i) << "\t" << setprecision(30) << qmaxwell.wts(i)
         << endl;
  }
  fout.close();

  // test integration
  cout << "evaluate integral: \\int_0^{2pi} \\int_0^\\infty e^{-r^} r \\dd r"
       << "\n";
  double sum = 0;
  for (int i = 0; i < N; ++i) {
    sum += qmaxwell.wts(i);
  }
  const double pi = boost::math::constants::pi<double>();
  sum *= 2 * pi;
  cout << "\terror:" << std::abs(sum - pi) << "\n";

  return 0;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef ITL_QUASI_NEWTON_INCLUDE
#define ITL_QUASI_NEWTON_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/operators.hpp>
#include <boost/numeric/mtl/operation/operators.hpp>
#include <boost/numeric/mtl/operation/trans.hpp>
#include <boost/numeric/mtl/utility/gradient.hpp>

// #include <iostream>

namespace itl {

/// Quasi-Newton method
template <typename Matrix, typename Vector, typename F, typename Grad,
	  typename Step, typename Update, typename Iter>
Vector quasi_newton(Vector& x, F f, Grad grad_f, Step step, Update update, Iter& iter)
{
    typedef typename mtl::Collection<Vector>::value_type value_type;
    Vector         d, y, x_k, s;
    Matrix         H(size(x), size(x));

    H= 1;
    for (; !iter.finished(two_norm(grad_f(x))); ++iter) {
	d= H * -grad_f(x);                                                  // std::cout << "d is " << d << '\n';
	value_type alpha= step(x, d, f, grad_f); assert(alpha == alpha);
	x_k= x + alpha * d;                                                 // std::cout << "x_k is " << x_k << '\n';
	s= alpha * d;                                                       // std::cout << "alpha is " << alpha << '\n';
	y= grad_f(x_k) - grad_f(x);
	update(H, y, s);
	x= x_k;
    }
    return x;
}

/// Quasi-Newton method
template <typename Vector, typename F, typename Grad, typename Step, typename Update, typename Iter>
Vector inline quasi_newton(Vector& x, F f, Grad grad_f, Step step, Update update, Iter& iter)
{
    typedef typename mtl::traits::gradient<Vector>::type hessian_type;
    // typedef typename mtl::Collection<Vector>::value_type value_type;
    return quasi_newton<hessian_type>(x, f, grad_f, step, update, iter);
}

} // namespace itl

#endif // ITL_QUASI_NEWTON_INCLUDE


//
// Created by a.kiryanenko on 3/26/20.
//

#include "../SpuUltraGraphAdapter.h"
#include "../SpuUltraGraphProperty.h"
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include "GraphPerformanceTest.h"

using namespace SPU_GRAPH;
using namespace boost;

typedef boost::adjacency_list <
        boost::vecS, // как хранить вершины - в векторе
        boost::vecS, // как хранить ребра из каждой вершины - в векторе
        boost::directedS,
        no_property,
        property < edge_weight_t, int >
> AdjacencyListGraph;

template <class G>
void breadth_first_test(G &g) {
    // Выполняю алгоритм поиска в глубину
    breadth_first_search(g, 1, visitor(default_bfs_visitor()));
}

int main()
{
    cout << "SpuUltraGraph performance test" << endl;
    cout << "==========================================" << endl;
    GraphPerformanceTest<SpuUltraGraph> spu_graph_test(breadth_first_test, "breadth_first_test_SpuUltraGraph.csv");
    spu_graph_test.is_mutable_test = false;
    spu_graph_test.start();

    cout << "adjacency_list performance test" << endl;
    cout << "==========================================" << endl;
    GraphPerformanceTest<AdjacencyListGraph> adjacency_list_test(breadth_first_test, "breadth_first_test_adjacency_list.csv");
    adjacency_list_test.is_mutable_test = false;
    adjacency_list_test.start();

    cout << "adjacency_matrix performance test" << endl;
    cout << "==========================================" << endl;
    GraphPerformanceTest<AdjacencyMatrixGraph> adjacency_matrix_test(breadth_first_test, "breadth_first_test_adjacency_matrix.csv");
    adjacency_matrix_test.is_mutable_test = false;
    adjacency_matrix_test.end_vertices_cnt = 20000;
    adjacency_matrix_test.start();
    return 0;
}


#ifndef slic3r_Point_hpp_
#define slic3r_Point_hpp_

#include <myinit.h>
#include <vector>
#include <math.h>
#include <boost/polygon/polygon.hpp>
#include <string>

namespace Slic3r {

class Line;
class Point;
class Pointf;
typedef Point Vector;
typedef std::vector<Point> Points;
typedef std::vector<Point*> PointPtrs;
typedef std::vector<Pointf> Pointfs;

class Point
{
    public:
    coord_t x;
    coord_t y;
    explicit Point(coord_t _x = 0, coord_t _y = 0): x(_x), y(_y) {};
    bool operator==(const Point& rhs) const;
    std::string wkt() const;
    void scale(double factor);
    void translate(double x, double y);
    void rotate(double angle, Point* center);
    bool coincides_with(const Point &point) const;
    bool coincides_with(const Point* point) const;
    int nearest_point_index(Points &points) const;
    int nearest_point_index(PointPtrs &points) const;
    Point* nearest_point(Points points) const;
    double distance_to(const Point* point) const;
    double distance_to(const Line* line) const;
    double distance_to(const Line &line) const;
    double ccw(const Point &p1, const Point &p2) const;
    double ccw(const Point* p1, const Point* p2) const;
    double ccw(const Line &line) const;

    #ifdef SLIC3RXS
    void from_SV(SV* point_sv);
    void from_SV_check(SV* point_sv);
    SV* to_SV_ref();
    SV* to_SV_clone_ref() const;
    SV* to_SV_pureperl() const;
    #endif
};

class Point3 : public Point
{
    public:
    coord_t z;
    explicit Point3(coord_t _x = 0, coord_t _y = 0, coord_t _z = 0): Point(_x, _y), z(_z) {};
};

class Pointf
{
    public:
    coordf_t x;
    coordf_t y;
    explicit Pointf(coordf_t _x = 0, coordf_t _y = 0): x(_x), y(_y) {};
    void scale(double factor);
    void translate(double x, double y);

    #ifdef SLIC3RXS
    bool from_SV(SV* point_sv);
    SV* to_SV_pureperl() const;
    #endif
};

class Pointf3 : public Pointf
{
    public:
    coordf_t z;
    explicit Pointf3(coordf_t _x = 0, coordf_t _y = 0, coordf_t _z = 0): Pointf(_x, _y), z(_z) {};
    void scale(double factor);
    void translate(double x, double y, double z);
};

}

// start Boost
namespace boost { namespace polygon {
    template <>
    struct geometry_concept<Point> { typedef point_concept type; };

    template <>
    struct point_traits<Point> {
        typedef coord_t coordinate_type;

        static inline coordinate_type get(const Point& point, orientation_2d orient) {
            return (orient == HORIZONTAL) ? point.x : point.y;
        }
    };
} }
// end Boost

#endif


/* test_geometric.cpp
 *
 * Copyright Steven Watanabe 2011
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * $Id$
 *
 */

#include <boost/random/geometric_distribution.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/math/distributions/geometric.hpp>
#include <boost/numeric/conversion/cast.hpp>

#define BOOST_RANDOM_DISTRIBUTION boost::random::geometric_distribution<>
#define BOOST_RANDOM_DISTRIBUTION_NAME geometric
#define BOOST_MATH_DISTRIBUTION boost::math::geometric
#define BOOST_RANDOM_ARG1_TYPE double
#define BOOST_RANDOM_ARG1_NAME p
#define BOOST_RANDOM_ARG1_DEFAULT 0.5
#define BOOST_RANDOM_ARG1_DISTRIBUTION(n) boost::uniform_real<>(0.0001, 0.9999)
#define BOOST_RANDOM_DISTRIBUTION_MAX boost::numeric_cast<int>(-5 / std::log(1-p))

#include "test_real_distribution.ipp"


/**
 *  @file    UtilsTest.cpp
 *  @brief   Test the utility functions.
 *  @author  Francois Roy
 *  @date    12/01/2019
 */
#include <catch2/catch.hpp>
#include <Eigen/Core>
#include "utils/Utils.hpp"

namespace
{

// true test
bool all_close_true(){
    Eigen::VectorXd u = Eigen::VectorXd::Constant(11, 1.2);
    Eigen::VectorXd d = Eigen::VectorXd::Constant(11, 1.2);
    return utils::all_close(u, d);
}

// false test
bool all_close_false(){
    Eigen::VectorXd u = Eigen::VectorXd::Constant(11, 1.2);
    Eigen::VectorXd d = Eigen::VectorXd::Constant(11, 1.3);
    return utils::all_close(u, d);
}

}  // namespace

TEST_CASE( "all_close are computed", "[all_close]" )
{
    CHECK( all_close_true() );
    CHECK_FALSE( all_close_false() );
}
TEST_CASE("Test linear_spaced", "[utils]")
{
	std::vector<double> u = {0.0, 1.1, 2.2, 3.3};
	auto v = utils::linear_spaced<double>(0, 3.3, 4);
	for(int i; i<u.size(); i++){
        CHECK(v[i] == Approx(u[i]) );
	}
}


#ifndef BLOCKSVD_H
#define BLOCKSVD_H

#include "clear/MyGraph.hpp"

#include <iostream>
#include <vector>
#include <numeric>
#include <queue>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>

using namespace std;
using Eigen::MatrixXf;
using Eigen::MatrixXi;

inline void blockSVD(Eigen::MatrixXf A, Eigen::MatrixXf Lnrm, Eigen::MatrixXf& Vl, Eigen::MatrixXf& sl)
{
	MyGraph G(A);
	unsigned n = A.rows();
	G.findConnComps();
	vector<vector<unsigned>> ConnComps = G.getConnComps();

	MatrixXf V;
	V = MatrixXf::Zero(n,n);
	vector<double> s(n, 0.0);
	// cout << "Number of connected components: " << ConnComps.size() << endl;

	for (unsigned cc = 0; cc < ConnComps.size(); ++cc){
		vector<unsigned> Comp = ConnComps[cc];
		unsigned CompSize = Comp.size();
		MatrixXf Lc;
		Lc = MatrixXf::Zero(CompSize, CompSize);
		for (unsigned i = 0; i < CompSize; ++i){
			for (unsigned j = 0; j < CompSize; ++j){
				Lc(i,j) = Lnrm(Comp[i], Comp[j]);
			}
		}
		// Compute SVD of this block
		Eigen::JacobiSVD<MatrixXf> svd(Lc, Eigen::ComputeFullU | Eigen::ComputeFullV);
		MatrixXf Vc = svd.matrixV();
		MatrixXf sc = svd.singularValues(); // a column vector of singular values in decreasing order

		for (unsigned i = 0; i < CompSize; ++i){
			for (unsigned j = 0; j < CompSize; ++j){
				V(Comp[i], Comp[j]) = Vc(i,j);
			}
		}
		for (unsigned i = 0; i < CompSize; ++i){
			s[Comp[i]] = sc(i);
		}
	}

	// sort based on singular values (in decreasing order)
	vector<size_t> sorted_idx(s.size());
	std::iota(sorted_idx.begin(), sorted_idx.end(), 0);
	std::sort(sorted_idx.begin(), sorted_idx.end(),
	       [&s](size_t i1, size_t i2) {return s[i1] > s[i2];});

	sl = MatrixXf::Zero(n,1);
	Vl = MatrixXf::Zero(n,n);

	// rearrange based on sorted index
	for (unsigned i = 0 ; i < n ; ++i){
		unsigned k = sorted_idx[i];
		sl(i) = s[k];
		Vl.col(i) = V.col(k);
	}
}

#endif


#include <iostream>
#include <vector>

#include <boost/range/adaptors.hpp>
namespace ba = boost::adaptors;

#include <dionysus/simplex.h>
#include <dionysus/filtration.h>
#include <dionysus/omni-field-persistence.h>
#include <dionysus/diagram.h>

namespace d = dionysus;

#include <format.h>

using Simplex       = d::Simplex<>;
using Filtration    = d::Filtration<Simplex>;
using Persistence   = d::OmniFieldPersistence<>;

int main()
{
    // Klein bottle
    Filtration filtration
    {
      {0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8},
      {0,1}, {1,2}, {2,0}, {0,3}, {3,4}, {4,0},
      {1,5}, {5,6}, {6,2}, {2,7}, {7,8}, {8,1},
      {3,5}, {5,7}, {7,3}, {4,6}, {6,8}, {8,4},
      {0,5}, {1,7}, {2,3}, {3,6}, {5,8}, {7,4},
      {4,2}, {6,1}, {8,0},
      {0,3,5}, {0,1,5}, {1,5,7}, {1,2,7}, {2,7,3}, {2,3,0},
      {3,4,6}, {3,5,6}, {5,6,8}, {5,7,8}, {7,8,4}, {7,3,4},
      {4,0,2}, {4,6,2}, {6,2,1}, {6,8,1}, {8,1,0}, {8,4,0}
    };

    fmt::print("Boundary matrix over Q\n");
    d::Q<> q;
    for (auto& s : filtration)
    {
        fmt::print("{} at {}\n", s, filtration.index(s));
        for (auto sb : s.boundary(q))
            fmt::print("   {} * {} at {}\n", sb.element(), sb.index(), filtration.index(sb.index()));
    }

    Persistence     persistence;
    for(auto& s : filtration)
    {
        using SimplexChainEntry = d::ChainEntry<Persistence::Field, Simplex>;
        using ChainEntry        = d::ChainEntry<Persistence::Field, Persistence::Index>;
        persistence.add(s.boundary(persistence.field()) |
                                                 ba::transformed([&filtration](const SimplexChainEntry& e)
                                                 { return ChainEntry(e.element(), filtration.index(e.index())); }));
    }
    fmt::print("Reduction finished\n");

    fmt::print("Special primes:");
    for (auto x : persistence.primes())
        fmt::print(" {}", x);
    fmt::print("\n");

    unsigned i = 0;
    fmt::print("Q chains finished\n");
    for (auto& c : persistence.q_chains())
    {
        fmt::print("{}: ", i);
        for (auto& ce : c)
            fmt::print(" + {} * {}", ce.element(), ce.index());
        fmt::print("\n");
        ++i;
    }

    fmt::print("Zp chains finished\n");
    for (auto& x : persistence.zp_chains())
    {
        unsigned i = x.first;
        fmt::print("{}:\n", i);
        for (auto& ec : x.second)
        {
            auto& e = ec.first;
            auto& c = ec.second;

            fmt::print("  mod {}:", e);

            for (auto& ce : c)
                fmt::print(" + {} * {}", ce.element(), ce.index());
        }
        fmt::print("\n");
    }

    auto primes = persistence.primes();
    primes.emplace(primes.begin(), 1);
    for (auto& p : primes)
    {
        if (p == 1)
            fmt::print("Over Z_p (for all p, except those specified explicitly)\n");
        else
            fmt::print("Over Z_{}:\n", p);
        auto diagrams = init_diagrams(prime_adapter(persistence, p), filtration,
                                      [&](const Simplex& s) -> float  { return filtration.index(s); },        // inefficient, but works
                                      [](Persistence::Index i)        { return i; });
        i = 0;
        for (auto& dgm : diagrams)
        {
            fmt::print("  Dimension {}:\n", i++);
            for (auto& pt : dgm)
                fmt::print("    {} {}\n", pt.birth(), pt.death());
        }
    }
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE rationalClass
#include <boost/test/unit_test.hpp>

#include "rational.h"

using namespace ExactArithmetic;
namespace utf = boost::unit_test;

// Test suite: arithmetic_operations
BOOST_AUTO_TEST_SUITE(arithmetic_operations)
BOOST_AUTO_TEST_CASE(addition)
{
  BOOST_CHECK_EQUAL(Rational(1, 3) + Rational(1, 3), Rational(2, 3));
}

BOOST_AUTO_TEST_CASE(subtraction)
{
  BOOST_CHECK_EQUAL(Rational(1, 3) - Rational(1, 3), Rational(0, 1));
}

BOOST_AUTO_TEST_CASE(multiplication)
{
  BOOST_CHECK_EQUAL(Rational(1, 3) * Rational(1, 3), Rational(1, 9));
}

BOOST_AUTO_TEST_CASE(division)
{
  BOOST_CHECK_EQUAL(Rational(1, 3) / Rational(1, 3), Rational(1, 1));
}
BOOST_AUTO_TEST_SUITE_END()
// End of test suite: arithmetic operations

// Test suite: constructors
BOOST_AUTO_TEST_SUITE(constructors)

BOOST_AUTO_TEST_CASE(default_constructor)
{
  // Test default constructor
  Rational defaultConstructor;
  BOOST_CHECK_EQUAL(defaultConstructor, Rational(0, 1));
}

BOOST_AUTO_TEST_CASE(two_thirds)
{
  // Test a num/denom constructor
  Rational twoThirds(2, 3);
  BOOST_CHECK_EQUAL(twoThirds, Rational(2, 3));
}

BOOST_AUTO_TEST_CASE(four_eight)
{
  // Test normalization
  Rational fourEight(4, 8);
  BOOST_CHECK_EQUAL(fourEight, Rational(1, 2));
}

BOOST_AUTO_TEST_CASE(integer_constructor)
{
  // Test integer constructor
  Rational integerConstructor(4);
  BOOST_CHECK_EQUAL(integerConstructor, Rational(4, 1));
}

BOOST_AUTO_TEST_CASE(double_constructor)
{
  // Test double constructor
  Rational doubleConstructor(4.0);
  BOOST_CHECK_EQUAL(doubleConstructor, Rational(4, 1));
}

BOOST_AUTO_TEST_SUITE_END()
// End of test suite: constructors

// Test suite: gcd
BOOST_AUTO_TEST_SUITE(gcd)
BOOST_AUTO_TEST_CASE(obvious)
{
  Rational veryBigRationalNumber(1000000000000000000, 2000000000000000000);
  BOOST_CHECK_EQUAL(veryBigRationalNumber, Rational(1, 2));
}

BOOST_AUTO_TEST_CASE(random)
{
  Rational veryBigRationalNumber(98822773366119922, 22773399118822);
  BOOST_CHECK_EQUAL(veryBigRationalNumber, Rational(49411386683059961, 11386699559411));
}
BOOST_AUTO_TEST_SUITE_END()
// End of test suite: gcd

BOOST_AUTO_TEST_SUITE(miscellaneous)
BOOST_AUTO_TEST_CASE(abs_function)
{
  Rational negativeOneTwo(-1, 2);
  BOOST_CHECK_EQUAL(Rational::abs(negativeOneTwo), Rational(1, 2));
}

BOOST_AUTO_TEST_CASE(negative)
{
  Rational negativeOneTwo(-1, 2);
  Rational negativeOneTwoOnDenom(1, -2);
  BOOST_CHECK_EQUAL(negativeOneTwo, Rational(-1, 2));
  BOOST_CHECK_EQUAL(negativeOneTwoOnDenom, Rational(-1, 2));
}

BOOST_AUTO_TEST_CASE(negate_function)
{
  Rational negativeOneTwo(-1, 2);
  Rational negativeOneTwoOnDenom(1, -2);

  Rational oneTwo(1, 2);

  BOOST_CHECK_EQUAL(Rational::negate(negativeOneTwo), Rational(1, 2));
  BOOST_CHECK_EQUAL(Rational::negate(negativeOneTwoOnDenom), Rational(1, 2));
  BOOST_CHECK_EQUAL(Rational::negate(oneTwo), Rational(-1, 2));
}
BOOST_AUTO_TEST_SUITE_END()


// distribution_construction.cpp

// Copyright Paul A. Bristow 2007, 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)

// Caution: this file contains Quickbook markup as well as code
// and comments, don't change any of the special comment markups!

//[distribution_construction1

/*`

The structure of distributions is rather different from some other statistical libraries,
for example in less object-oriented language like FORTRAN and C,
that provide a few arguments to each free function.
This library provides each distribution as a template C++ class.
A distribution is constructed with a few arguments, and then
member and non-member functions are used to find values of the
distribution, often a function of a random variate.

First we need some includes to access the negative binomial distribution
(and the binomial, beta and gamma too).

*/

#include <boost/math/distributions/negative_binomial.hpp> // for negative_binomial_distribution
  using boost::math::negative_binomial_distribution; // default type is double.
  using boost::math::negative_binomial; // typedef provides default type is double.
#include <boost/math/distributions/binomial.hpp> // for binomial_distribution.
#include <boost/math/distributions/beta.hpp> // for beta_distribution.
#include <boost/math/distributions/gamma.hpp> // for gamma_distribution.
#include <boost/math/distributions/normal.hpp> // for normal_distribution.
/*`
Several examples of constructing distributions follow:
*/
//] [/distribution_construction1 end of Quickbook in C++ markup]

int main()
{
//[distribution_construction2
/*`
First, a negative binomial distribution with 8 successes
and a success fraction 0.25, 25% or 1 in 4, is constructed like this:
*/
  boost::math::negative_binomial_distribution<double> mydist0(8., 0.25);
  /*`
  But this is inconveniently long, so we might be tempted to write
  */
  using namespace boost::math;
  /*`
  but this might risk ambiguity with names in std random so
  *much better is explicit `using boost::math:: ` * ... statements like
  */
  using boost::math::negative_binomial_distribution;
  /*`
  and we can still reduce typing.

  Since the vast majority of applications use will be using double precision,
  the template argument to the distribution (RealType) defaults
  to type double, so we can also write:
  */

  negative_binomial_distribution<> mydist9(8., 0.25); // Uses default RealType = double.

  /*`
  But the name "negative_binomial_distribution" is still inconveniently long,
  so for most distributions, a convenience typedef is provided, for example:

     typedef negative_binomial_distribution<double> negative_binomial; // Reserved name of type double.

  [caution
  This convenience typedef is /not/ provided if a clash would occur
  with the name of a function: currently only "beta" and "gamma"
  fall into this category.
  ]

  So, after a using statement,
  */

  using boost::math::negative_binomial;

  /*`
  we have a convenient typedef to `negative_binomial_distribution<double>`:
  */
  negative_binomial mydist(8., 0.25);

  /*`
  Some more examples using the convenience typedef:
  */
  negative_binomial mydist10(5., 0.4); // Both arguments double.
  /*`
  And automatic conversion takes place, so you can use integers and floats:
  */
  negative_binomial mydist11(5, 0.4); // Using provided typedef double, int and double arguments.
  /*`
  This is probably the most common usage.
  */
  negative_binomial mydist12(5., 0.4F); // Double and float arguments.
  negative_binomial mydist13(5, 1); // Both arguments integer.

  /*`
  Similarly for most other distributions like the binomial.
  */
  binomial mybinomial(1, 0.5); // is more concise than
  binomial_distribution<> mybinomd1(1, 0.5);

  /*`
  For cases when the typdef distribution name would clash with a math special function
  (currently only beta and gamma)
  the typedef is deliberately not provided, and the longer version of the name
  must be used.  For example do not use:

     using boost::math::beta;
     beta mybetad0(1, 0.5); // Error beta is a math FUNCTION!

  Which produces the error messages:

  [pre
  error C2146: syntax error : missing ';' before identifier 'mybetad0'
  warning C4551: function call missing argument list
  error C3861: 'mybetad0': identifier not found
  ]

  Instead you should use:
  */
  using boost::math::beta_distribution;
  beta_distribution<> mybetad1(1, 0.5);
  /*`
  or for the gamma distribution:
  */
  gamma_distribution<> mygammad1(1, 0.5);

  /*`
  We can, of course, still provide the type explicitly thus:
  */

  // Explicit double precision:
  negative_binomial_distribution<double>        mydist1(8., 0.25);

  // Explicit float precision, double arguments are truncated to float:
  negative_binomial_distribution<float>         mydist2(8., 0.25);

  // Explicit float precision, integer & double arguments converted to float.
  negative_binomial_distribution<float>         mydist3(8, 0.25);

  // Explicit float precision, float arguments, so no conversion:
  negative_binomial_distribution<float>         mydist4(8.F, 0.25F);

  // Explicit float precision, integer arguments promoted to float.
  negative_binomial_distribution<float>         mydist5(8, 1);

  // Explicit double precision:
  negative_binomial_distribution<double>        mydist6(8., 0.25);

  // Explicit long double precision:
  negative_binomial_distribution<long double>   mydist7(8., 0.25);

  /*`
  And if you have your own RealType called MyFPType,
  for example NTL RR (an arbitrary precision type), then we can write:

     negative_binomial_distribution<MyFPType>  mydist6(8, 1); // Integer arguments -> MyFPType.

  [heading Default arguments to distribution constructors.]

  Note that default constructor arguments are only provided for some distributions.
  So if you wrongly assume a default argument you will get an error message, for example:

     negative_binomial_distribution<> mydist8;

  [pre error C2512 no appropriate default constructor available.]

  No default constructors are provided for the negative binomial,
  because it is difficult to chose any sensible default values for this distribution.
  For other distributions, like the normal distribution,
  it is obviously very useful to provide 'standard'
  defaults for the mean and standard deviation thus:

      normal_distribution(RealType mean = 0, RealType sd = 1);

  So in this case we can write:
  */
  using boost::math::normal;

  normal norm1;       // Standard normal distribution.
  normal norm2(2);    // Mean = 2, std deviation = 1.
  normal norm3(2, 3); // Mean = 2, std deviation = 3.

  return 0;
}  // int main()

/*`There is no useful output from this program, of course. */

//] [/end of distribution_construction2]