/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file   testExtendedPose3.cpp
 * @brief  Unit tests for ExtendedPose3 class
 */

#include <gtsam/geometry/ExtendedPose3.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/lieProxies.h>

#include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign;

#include <CppUnitLite/TestHarness.h>
#include <cmath>

using namespace std;
using namespace gtsam;

GTSAM_CONCEPT_TESTABLE_INST(ExtendedPose3)
GTSAM_CONCEPT_LIE_INST(ExtendedPose3)

static const Point3 V(3,0.4,-2.2);
static const Point3 P(0.2,0.7,-2);
static const Rot3 R = Rot3::Rodrigues(0.3,0,0);
static const Point3 V2(-6.5,3.5,6.2);
static const Point3 P2(3.5,-8.2,4.2);
static const ExtendedPose3 T(R,V2,P2);
static const ExtendedPose3 T2(Rot3::Rodrigues(0.3,0.2,0.1),V2,P2);
static const ExtendedPose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(5,6,7), Point3(1, 2, 3));
static const double tol=1e-5;

/* ************************************************************************* */
TEST( ExtendedPose3, equals)
{
  ExtendedPose3 pose2 = T3;
  EXPECT(T3.equals(pose2));
  ExtendedPose3 origin;
  EXPECT(!T3.equals(origin));
}

/* ************************************************************************* */
#ifndef GTSAM_POSE3_EXPMAP
TEST( ExtendedPose3, retract_first_order)
{
  ExtendedPose3 id;
  Vector xi = Z_9x1;
  xi(0) = 0.3;
  EXPECT(assert_equal(ExtendedPose3(R, Vector3(0,0,0), Point3(0,0,0)), id.retract(xi),1e-2));
  xi(3)=3;xi(4)=0.4;xi(5)=-2.2;
  xi(6)=0.2;xi(7)=0.7;xi(8)=-2;
  EXPECT(assert_equal(ExtendedPose3(R, V, P),id.retract(v),1e-2));
}
#endif
/* ************************************************************************* */
TEST( ExtendedPose3, retract_expmap)
{
  Vector xi = Z_9x1; xi(0) = 0.3;
  ExtendedPose3 pose = ExtendedPose3::Expmap(xi);
  EXPECT(assert_equal(ExtendedPose3(R, Point3(0,0,0), Point3(0,0,0)), pose, 1e-2));
  EXPECT(assert_equal(xi,ExtendedPose3::Logmap(pose),1e-2));
}

/* ************************************************************************* */
TEST( ExtendedPose3, expmap_a_full)
{
  ExtendedPose3 id;
  Vector xi = Z_9x1;
  xi(0) = 0.3;
  EXPECT(assert_equal(expmap_default<ExtendedPose3>(id, xi), ExtendedPose3(R, Vector3(0,0,0), Point3(0,0,0))));
  xi(3)=-0.2;xi(4)=-0.394742;xi(5)=2.08998;
  xi(6)=0.2;xi(7)=0.394742;xi(8)=-2.08998;
  EXPECT(assert_equal(ExtendedPose3(R, -P, P),expmap_default<ExtendedPose3>(id, xi),1e-5));
}

/* ************************************************************************* */
TEST( ExtendedPose3, expmap_a_full2)
{
  ExtendedPose3 id;
  Vector xi = Z_9x1;
  xi(0) = 0.3;
  EXPECT(assert_equal(expmap_default<ExtendedPose3>(id, xi), ExtendedPose3(R, Point3(0,0,0), Point3(0,0,0))));
  xi(3)=-0.2;xi(4)=-0.394742;xi(5)=2.08998;
  xi(6)=0.2;xi(7)=0.394742;xi(8)=-2.08998;
  EXPECT(assert_equal(ExtendedPose3(R, -P, P),expmap_default<ExtendedPose3>(id, xi),1e-5));
}

/* ************************************************************************* */
TEST(ExtendedPose3, expmap_b)
{
  ExtendedPose3 p1(Rot3(), Vector3(-100, 0, 0), Point3(100, 0, 0));
  ExtendedPose3 p2 = p1.retract((Vector(9) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished());
  ExtendedPose3 expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(-100.0, 0.0, 0.0), Point3(100.0, 0.0, 0.0));
  EXPECT(assert_equal(expected, p2,1e-2));
}

/* ************************************************************************* */
// test case for screw motion in the plane
namespace screwExtendedPose3 {
  double a=0.3, c=cos(a), s=sin(a), w=0.3;
  Vector xi = (Vector(9) << 0.0, 0.0, w, w, 0.0, 1.0, w, 0.0, 1.0).finished();
  Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1);
  Point3 expectedV(0.29552, 0.0446635, 1);
  Point3 expectedP(0.29552, 0.0446635, 1);
  ExtendedPose3 expected(expectedR, expectedV, expectedP);
}

/* ************************************************************************* */
// Checks correct exponential map (Expmap) with brute force matrix exponential
TEST(ExtendedPose3, expmap_c_full)
{
  EXPECT(assert_equal(screwExtendedPose3::expected, expm<ExtendedPose3>(screwExtendedPose3::xi),1e-6));
  EXPECT(assert_equal(screwExtendedPose3::expected, ExtendedPose3::Expmap(screwExtendedPose3::xi),1e-6));
}

/* ************************************************************************* */
// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
TEST(ExtendedPose3, Adjoint_full)
{
  ExtendedPose3 expected = T * ExtendedPose3::Expmap( screwExtendedPose3::xi) * T.inverse();
  Vector xiprime = T.Adjoint( screwExtendedPose3::xi);
  EXPECT(assert_equal(expected, ExtendedPose3::Expmap(xiprime), 1e-6));

  ExtendedPose3 expected2 = T2 * ExtendedPose3::Expmap( screwExtendedPose3::xi) * T2.inverse();
  Vector xiprime2 = T2.Adjoint( screwExtendedPose3::xi);
  EXPECT(assert_equal(expected2, ExtendedPose3::Expmap(xiprime2), 1e-6));

  ExtendedPose3 expected3 = T3 * ExtendedPose3::Expmap( screwExtendedPose3::xi) * T3.inverse();
  Vector xiprime3 = T3.Adjoint( screwExtendedPose3::xi);
  EXPECT(assert_equal(expected3, ExtendedPose3::Expmap(xiprime3), 1e-6));
}

/* ************************************************************************* */
// assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
TEST(ExtendedPose3, Adjoint_hat)
{
  auto hat = [](const Vector& xi) { return ::wedge<ExtendedPose3>(xi); };
  Matrix5 expected = T.matrix() * hat( screwExtendedPose3::xi) * T.matrix().inverse();
  Matrix5 xiprime = hat(T.Adjoint( screwExtendedPose3::xi));

  EXPECT(assert_equal(expected, xiprime, 1e-6));

  Matrix5 expected2 = T2.matrix() * hat( screwExtendedPose3::xi) * T2.matrix().inverse();
  Matrix5 xiprime2 = hat(T2.Adjoint( screwExtendedPose3::xi));
  EXPECT(assert_equal(expected2, xiprime2, 1e-6));

  Matrix5 expected3 = T3.matrix() * hat( screwExtendedPose3::xi) * T3.matrix().inverse();

  Matrix5 xiprime3 = hat(T3.Adjoint( screwExtendedPose3::xi));
  EXPECT(assert_equal(expected3, xiprime3, 1e-6));
}

/* ************************************************************************* */
TEST(ExtendedPose3, expmaps_galore_full)
{
  Vector xi; ExtendedPose3 actual;
  xi = (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished();
  actual = ExtendedPose3::Expmap(xi);
  EXPECT(assert_equal(expm<ExtendedPose3>(xi), actual,1e-6));
  EXPECT(assert_equal(xi, ExtendedPose3::Logmap(actual),1e-6));

  xi = (Vector(9) << 0.1, -0.2, 0.3, -0.4, 0.5, -0.6, -0.7, -0.8, -0.9).finished();
  for (double theta=1.0;0.3*theta<=M_PI;theta*=2) {
    Vector txi = xi*theta;
    actual = ExtendedPose3::Expmap(txi);
    EXPECT(assert_equal(expm<ExtendedPose3>(txi,30), actual,1e-6));
    Vector log = ExtendedPose3::Logmap(actual);
    EXPECT(assert_equal(actual, ExtendedPose3::Expmap(log),1e-6));
    EXPECT(assert_equal(txi,log,1e-6)); // not true once wraps
  }

  // Works with large v as well, but expm needs 10 iterations!
  xi = (Vector(9) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0, 12, 14, 45).finished();
  actual = ExtendedPose3::Expmap(xi);
  EXPECT(assert_equal(expm<ExtendedPose3>(xi,10), actual,1e-5));
  EXPECT(assert_equal(xi, ExtendedPose3::Logmap(actual),1e-9));
}

/* ************************************************************************* */
// Check position and its pushforward

TEST(ExtendedPose3, position) {
  Matrix actualH;
  EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.position(actualH), 1e-8));
  Matrix numericalH = numericalDerivative11<Point3, ExtendedPose3>(
      boost::bind(&ExtendedPose3::position, _1, boost::none), T);
  EXPECT(assert_equal(numericalH, actualH, 1e-6));
}

/* ************************************************************************* */
// Check rotation and its pushforward
TEST(ExtendedPose3, rotation) {
  Matrix actualH;
  EXPECT(assert_equal(R, T.rotation(actualH), 1e-8));

  Matrix numericalH = numericalDerivative11<Rot3, ExtendedPose3>(
      boost::bind(&ExtendedPose3::rotation, _1, boost::none), T);
  EXPECT(assert_equal(numericalH, actualH, 1e-6));
}

/* ************************************************************************* */
// Check velocity and its pushforward
TEST(ExtendedPose3, velocity) {
  Matrix actualH;
  EXPECT(assert_equal(Point3(-6.5,3.5,6.2), T.velocity(actualH), 1e-8));
  Matrix numericalH = numericalDerivative11<Point3, ExtendedPose3>(
      boost::bind(&ExtendedPose3::velocity, _1, boost::none), T);
  EXPECT(assert_equal(numericalH, actualH, 1e-6));
}

/* ************************************************************************* */
TEST(ExtendedPose3, Adjoint_compose_full)
{
  // To debug derivatives of compose, assert that
  // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2
  const ExtendedPose3& T1 = T;
  Vector x = (Vector(9) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8, 0.4, 0.2, 0.8).finished();
  ExtendedPose3 expected = T1 * ExtendedPose3::Expmap(x) * T2;
  Vector y = T2.inverse().Adjoint(x);
  ExtendedPose3 actual = T1 * T2 * ExtendedPose3::Expmap(y);
  EXPECT(assert_equal(expected, actual, 1e-6));
}

/* ************************************************************************* */
// Check compose and its pushforward
// NOTE: testing::compose<ExtendedPose3>(t1,t2) = t1.compose(t2)  (see lieProxies.h)
TEST( ExtendedPose3, compose )
{
  Matrix actual = (T2*T2).matrix();

  Matrix expected = T2.matrix()*T2.matrix();
  EXPECT(assert_equal(actual,expected,1e-8));

  Matrix actualDcompose1, actualDcompose2;
  T2.compose(T2, actualDcompose1, actualDcompose2);

  Matrix numericalH1 = numericalDerivative21(testing::compose<ExtendedPose3>, T2, T2);

  EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
  EXPECT(assert_equal(T2.inverse().AdjointMap(),actualDcompose1,5e-3));

  Matrix numericalH2 = numericalDerivative22(testing::compose<ExtendedPose3>, T2, T2);
  EXPECT(assert_equal(numericalH2,actualDcompose2,1e-4));
}

/* ************************************************************************* */
// Check compose and its pushforward, another case
TEST( ExtendedPose3, compose2 )
{
  const ExtendedPose3& T1 = T;
  Matrix actual = (T1*T2).matrix();
  Matrix expected = T1.matrix()*T2.matrix();
  EXPECT(assert_equal(actual,expected,1e-8));

  Matrix actualDcompose1, actualDcompose2;
  T1.compose(T2, actualDcompose1, actualDcompose2);

  Matrix numericalH1 = numericalDerivative21(testing::compose<ExtendedPose3>, T1, T2);
  EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
  EXPECT(assert_equal(T2.inverse().AdjointMap(),actualDcompose1,5e-3));

  Matrix numericalH2 = numericalDerivative22(testing::compose<ExtendedPose3>, T1, T2);
  EXPECT(assert_equal(numericalH2,actualDcompose2,1e-5));
}

/* ************************************************************************* */
TEST( ExtendedPose3, inverse)
{
  Matrix actualDinverse;
  Matrix actual = T.inverse(actualDinverse).matrix();
  Matrix expected = T.matrix().inverse();
  EXPECT(assert_equal(actual,expected,1e-8));

  Matrix numericalH = numericalDerivative11(testing::inverse<ExtendedPose3>, T);
  EXPECT(assert_equal(numericalH,actualDinverse,5e-3));
  EXPECT(assert_equal(-T.AdjointMap(),actualDinverse,5e-3));
}

/* ************************************************************************* */
TEST( ExtendedPose3, inverseDerivatives2)
{
  Rot3 R = Rot3::Rodrigues(0.3,0.4,-0.5);
  Vector3 v(3.5,-8.2,4.2);
  Point3 p(3.5,-8.2,4.2);
  ExtendedPose3 T(R,v,p);

  Matrix numericalH = numericalDerivative11(testing::inverse<ExtendedPose3>, T);
  Matrix actualDinverse;
  T.inverse(actualDinverse);
  EXPECT(assert_equal(numericalH,actualDinverse,5e-3));
  EXPECT(assert_equal(-T.AdjointMap(),actualDinverse,5e-3));
}

/* ************************************************************************* */
TEST( ExtendedPose3, compose_inverse)
{
  Matrix actual = (T*T.inverse()).matrix();
  Matrix expected = I_5x5;
  EXPECT(assert_equal(actual,expected,1e-8));
}

/* ************************************************************************* */
TEST(ExtendedPose3, Retract_LocalCoordinates)
{
  Vector9 d;
  d << 1,2,3,4,5,6,7,8,9; d/=10;
  const Rot3 R = Rot3::Retract(d.head<3>());
  ExtendedPose3 t = ExtendedPose3::Retract(d);
  EXPECT(assert_equal(d, ExtendedPose3::LocalCoordinates(t)));
}
/* ************************************************************************* */
TEST(ExtendedPose3, retract_localCoordinates)
{
  Vector9 d12;
  d12 << 1,2,3,4,5,6,7,8,9; d12/=10;
  ExtendedPose3 t1 = T, t2 = t1.retract(d12);
  EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
}
/* ************************************************************************* */
TEST(ExtendedPose3, expmap_logmap)
{
  Vector d12 = Vector9::Constant(0.1);
  ExtendedPose3 t1 = T, t2 = t1.expmap(d12);
  EXPECT(assert_equal(d12, t1.logmap(t2)));
}

/* ************************************************************************* */
TEST(ExtendedPose3, retract_localCoordinates2)
{
  ExtendedPose3 t1 = T;
  ExtendedPose3 t2 = T3;
  ExtendedPose3 origin;
  Vector d12 = t1.localCoordinates(t2);
  EXPECT(assert_equal(t2, t1.retract(d12)));
  Vector d21 = t2.localCoordinates(t1);
  EXPECT(assert_equal(t1, t2.retract(d21)));
  EXPECT(assert_equal(d12, -d21));
}
/* ************************************************************************* */
TEST(ExtendedPose3, manifold_expmap)
{
  ExtendedPose3 t1 = T;
  ExtendedPose3 t2 = T3;
  ExtendedPose3 origin;
  Vector d12 = t1.logmap(t2);
  EXPECT(assert_equal(t2, t1.expmap(d12)));
  Vector d21 = t2.logmap(t1);
  EXPECT(assert_equal(t1, t2.expmap(d21)));

  // Check that log(t1,t2)=-log(t2,t1)
  EXPECT(assert_equal(d12,-d21));
}

/* ************************************************************************* */
TEST(ExtendedPose3, subgroups)
{
  // Frank - Below only works for correct "Agrawal06iros style expmap
  // lines in canonical coordinates correspond to Abelian subgroups in SE(3)
   Vector d = (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished();
  // exp(-d)=inverse(exp(d))
   EXPECT(assert_equal(ExtendedPose3::Expmap(-d),ExtendedPose3::Expmap(d).inverse()));
  // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
   ExtendedPose3 T2 = ExtendedPose3::Expmap(2*d);
   ExtendedPose3 T3 = ExtendedPose3::Expmap(3*d);
   ExtendedPose3 T5 = ExtendedPose3::Expmap(5*d);
   EXPECT(assert_equal(T5,T2*T3));
   EXPECT(assert_equal(T5,T3*T2));
}

/* ************************************************************************* */
TEST( ExtendedPose3, between )
{
  ExtendedPose3 expected = T2.inverse() * T3;
  Matrix actualDBetween1,actualDBetween2;
  ExtendedPose3 actual = T2.between(T3, actualDBetween1,actualDBetween2);
  EXPECT(assert_equal(expected,actual));

  Matrix numericalH1 = numericalDerivative21(testing::between<ExtendedPose3> , T2, T3);
  EXPECT(assert_equal(numericalH1,actualDBetween1,5e-3));

  Matrix numericalH2 = numericalDerivative22(testing::between<ExtendedPose3> , T2, T3);
  EXPECT(assert_equal(numericalH2,actualDBetween2,1e-5));
}

/* ************************************************************************* */
TEST( ExtendedPose3, adjointMap) {
  Matrix res = ExtendedPose3::adjointMap( screwExtendedPose3::xi);
  Matrix wh = skewSymmetric( screwExtendedPose3::xi(0),  screwExtendedPose3::xi(1),  screwExtendedPose3::xi(2));
  Matrix vh = skewSymmetric( screwExtendedPose3::xi(3),  screwExtendedPose3::xi(4),  screwExtendedPose3::xi(5));
  Matrix rh = skewSymmetric( screwExtendedPose3::xi(6),  screwExtendedPose3::xi(7),  screwExtendedPose3::xi(8));
  Matrix9 expected;
  expected << wh, Z_3x3, Z_3x3, vh, wh, Z_3x3, rh, Z_3x3, wh;
  EXPECT(assert_equal(expected,res,1e-5));
}

/* ************************************************************************* */

TEST( ExtendedPose3, ExpmapDerivative1) {
  Matrix9 actualH;
  Vector9 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0;
  ExtendedPose3::Expmap(w,actualH);
  Matrix expectedH = numericalDerivative21<ExtendedPose3, Vector9,
      OptionalJacobian<9, 9> >(&ExtendedPose3::Expmap, w, boost::none);
  EXPECT(assert_equal(expectedH, actualH));
}

/* ************************************************************************* */
TEST( ExtendedPose3, LogmapDerivative) {
  Matrix9 actualH;
  Vector9 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0,7.0,8.0,9.0;
  ExtendedPose3 p = ExtendedPose3::Expmap(w);
  EXPECT(assert_equal(w, ExtendedPose3::Logmap(p,actualH), 1e-5));
  Matrix expectedH = numericalDerivative21<Vector9, ExtendedPose3,
      OptionalJacobian<9, 9> >(&ExtendedPose3::Logmap, p, boost::none);
  EXPECT(assert_equal(expectedH, actualH));
}

/* ************************************************************************* */
TEST( ExtendedPose3, stream)
{
  ExtendedPose3 T;
  std::ostringstream os;
  os << T;
  EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\nv:[0, 0, 0];\np:[0, 0, 0];\n");
}

//******************************************************************************
TEST(ExtendedPose3 , Invariants) {
  ExtendedPose3 id;

  EXPECT(check_group_invariants(id,id));
  EXPECT(check_group_invariants(id,T3));
  EXPECT(check_group_invariants(T2,id));
  EXPECT(check_group_invariants(T2,T3));

  EXPECT(check_manifold_invariants(id,id));
  EXPECT(check_manifold_invariants(id,T3));
  EXPECT(check_manifold_invariants(T2,id));
  EXPECT(check_manifold_invariants(T2,T3));
}

//******************************************************************************
TEST(ExtendedPose3 , LieGroupDerivatives) {
  ExtendedPose3 id;

  CHECK_LIE_GROUP_DERIVATIVES(id,id);
  CHECK_LIE_GROUP_DERIVATIVES(id,T2);
  CHECK_LIE_GROUP_DERIVATIVES(T2,id);
  CHECK_LIE_GROUP_DERIVATIVES(T2,T3);
}

//******************************************************************************
TEST(ExtendedPose3 , ChartDerivatives) {
  ExtendedPose3 id;
  if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
    CHECK_CHART_DERIVATIVES(id,id);
//    CHECK_CHART_DERIVATIVES(id,T2);
//    CHECK_CHART_DERIVATIVES(T2,id);
//    CHECK_CHART_DERIVATIVES(T2,T3);
  }
}

/* ************************************************************************* */
TEST(ExtendedPose3, interpolate) {
  EXPECT(assert_equal(T2, interpolate(T2,T3, 0.0)));
  EXPECT(assert_equal(T3, interpolate(T2,T3, 1.0)));
}

/* ************************************************************************* */

TEST(ExtendedPose3, Create) {
  Matrix93 actualH1, actualH2, actualH3;
  ExtendedPose3 actual = ExtendedPose3::Create(R, V2, P2, actualH1, actualH2, actualH3);
  EXPECT(assert_equal(T, actual));
  boost::function<ExtendedPose3(Rot3,Point3,Point3)> create = boost::bind(ExtendedPose3::Create,_1,_2,_3,boost::none,boost::none,boost::none);
  EXPECT(assert_equal(numericalDerivative31<ExtendedPose3,Rot3,Point3,Point3>(create, R, V2, P2), actualH1, 1e-9));
  EXPECT(assert_equal(numericalDerivative32<ExtendedPose3,Rot3,Point3,Point3>(create, R, V2, P2), actualH2, 1e-9));
  EXPECT(assert_equal(numericalDerivative33<ExtendedPose3,Rot3,Point3,Point3>(create, R, V2, P2), actualH3, 1e-9));
}

/* ************************************************************************* */
TEST(ExtendedPose3, print) {
  std::stringstream redirectStream;
  std::streambuf* ssbuf = redirectStream.rdbuf();
  std::streambuf* oldbuf  = std::cout.rdbuf();
  // redirect cout to redirectStream
  std::cout.rdbuf(ssbuf);

  ExtendedPose3 pose(Rot3::identity(), Vector3(1, 2, 3), Point3(1, 2, 3));
  // output is captured to redirectStream
  pose.print();

  // Generate the expected output
  std::stringstream expected;
  Vector3 velocity(1, 2, 3);
  Point3 position(1, 2, 3);

#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
  expected << "1\n"
              "2\n"
              "3;\n";
#else
  expected << "v:[" << velocity.x() << ", " << velocity.y() << ", " << velocity.z() << "]';\np:[" << position.x() << ", " << position.y() << ", " << position.z() << "]';\n";
#endif

  // reset cout to the original stream
  std::cout.rdbuf(oldbuf);

  // Get substring corresponding to position part
  std::string actual = redirectStream.str().substr(38);
  CHECK_EQUAL(expected.str(), actual);
}

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


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_CONSTANTS_RADINDEG_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_CONSTANTS_RADINDEG_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>

namespace nt2
{
  namespace tag
  {
   /*!
     @brief Radindeg generic tag

     Represents the Radindeg constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Radindeg, double
                                , 57, 0x42652ee1
                                , 0x404ca5dc1a63c1f8ll
                                )
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Radindeg, Site> dispatching_Radindeg(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Radindeg, Site>();
   }
   template<class... Args>
   struct impl_Radindeg;
  }
  /*!
    Constant Radindeg : Degree in Radian multiplier, \f$\frac{180}\pi\f$.

    @par Semantic:

    For type T0:

    @code
    T0 r = Radindeg<T0>();
    @endcode

    is similar to:

    @code
    T0 r = _180<T0>()/Pi<T0>();
    @endcode

    @see  @funcref{inrad}, @funcref{indeg}, @funcref{Radindegr}, @funcref{Deginrad}
    @return a value of type T0
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Radindeg, Radindeg);
}

namespace nt2
{
  /// INTERNAL ONLY
}

#endif


#include <iostream>
#include <cassert>
#include <vector>
#include <boost/graph/adjacency_list.hpp>
#include <boost/iterator/function_output_iterator.hpp>
#include <boost/graph/biconnected_components.hpp>

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> Graph;

void testcase()
{
  int n, m;
  std::cin >> n >> m;
  assert(n >= 0 && n <= 30000 && m >= 0 && m <= 30000);

  Graph G(n + m);
  std::vector<std::pair<int, int>> edges;
  for (int i = 0; i < m; i++)
  {
    int start, end;
    std::cin >> start >> end;
    if (start > end)
    {
      std::swap(start, end);
    }
    assert(start >= 0 && start < n && end >= 0 && end < n && start < end);
    edges.push_back(std::make_pair(start, end));
    boost::add_edge(start, n + i, G);
    boost::add_edge(n + i, end, G);
  }

  std::vector<std::pair<int, int>> critical_bridges;
  auto on_articulation = [n, &critical_bridges, &edges](int vertex) {
    if (vertex >= n)
    {
      critical_bridges.push_back(edges.at(vertex - n));
    }
  };
  boost::articulation_points(G, boost::make_function_output_iterator(on_articulation));

  std::cout << critical_bridges.size() << "\n";
  std::sort(critical_bridges.begin(), critical_bridges.end());
  for (auto bridge : critical_bridges)
  {
    std::cout << bridge.first << " " << bridge.second << "\n";
  }
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
  }

  return 0;
}


#include <iostream>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>
#include <iomanip>
#include <Eigen/Dense>
#include <cmath>
#include <vector>
#include "mnistrec.h"

using namespace std;
using namespace MLP;


//  ================================================================
//  Created by Gregory Kramida on 10/23/18.
//  Copyright (c) 2018 Gregory Kramida
//  Licensed under the Apache License, Version 2.0 (the "License");
//  you may not use this file except in compliance with the License.
//  You may obtain a copy of the License at

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

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

#include <Eigen/Eigen>
namespace eig = Eigen;

namespace math{

//======================================================================================================================
//		                    Base Vector Structure (inspired by InfiniTAM ORUtils)
//======================================================================================================================

template <class T> struct Vector2_{
	union {
		struct { T x, y; }; // standard names for components
		struct { T s, t; }; // standard names for components
		struct { T u, v; }; // standard names for components
		struct { T width, height; }; // standard names for components
		T values[2];     // array access
	};
};

template <class T> struct Vector3_{
	union {
		struct{ T x, y, z; }; // standard names for components
		struct{ T r, g, b; }; // standard names for components
		struct{ T s, t, p; }; // standard names for components
		struct{ T u, v, w; }; // standard names for components
		T values[3];
	};
};

template <class T> struct Vector4_ {
	union {
		struct { T fx, fy, cx, cy; }; // names for components of a projection matrix
		struct { T x, y, z, w; }; // standard names for components
		struct { T r, g, b, a; }; // standard names for components
		struct { T s, t, p, q; }; // standard names for components
		struct { T u_x, u_y, v_x, v_y; }; // names for gradients of a vector field with u and v fields
		T values[4];
	};
};

template <class T> struct Vector6_ {
	union {
		struct { T min_x, min_y, min_z, max_x, max_y, max_z; };// standard names for components
		T values[6];
	};
};

template <class T> struct Vector9_ {
	union {
		struct { T u_x, u_y, u_z, v_x, v_y, v_z, w_x, w_y, w_z;};// names for gradients of a vector field with u, v, and w fields
		T values[9];
	};
};

template<class T, int s> struct VectorX_
{
	T values[s];
};

}


/**
 * @file transpsemilagr_test.cc
 * @brief NPDE homework TranspSemiLagr test file
 * @author Philippe Peter
 * @date November 2020
 * @copyright Developed at SAM, ETH Zurich
 */
#include "../transpsemilagr.h"

#include <gtest/gtest.h>
#include <lf/mesh/test_utils/test_meshes.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <cmath>
#include <memory>

namespace TranspSemiLagr::test {

// verifies that the vector u of nodal values satisfies zero boundary conditions
void verify_zero_bc(
    std::shared_ptr<const lf::uscalfe::UniformScalarFESpace<double>> fe_space,
    const Eigen::VectorXd& u) {
  auto mesh_p = fe_space->Mesh();
  const auto& dof_h = fe_space->LocGlobMap();
  auto boundary_nodes = lf::mesh::utils::flagEntitiesOnBoundary(mesh_p, 2);

  for (int i = 0; i < u.size(); ++i) {
    if (boundary_nodes(dof_h.Entity(i))) {
      EXPECT_NEAR(u[i], 0.0, 1.0E-6);
    }
  }
}

// The first set of test cases verifies, that all methods produce solutions that
// satisfy the zero dirichlet boundary conditions.

TEST(ReactionStep, boundary_conditions) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(4, 1.0);
  auto fe_space =
      std::make_shared<const lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  auto u_function = [](Eigen::VectorXd x) {
    return x(1) * x(0) * (1 - x(1)) * (1 - x(0));
  };
  Eigen::VectorXd u0_vector = lf::fe::NodalProjection(
      *fe_space, lf::mesh::utils::MeshFunctionGlobal(u_function));
  auto c_function = [](Eigen::VectorXd x) { return x(0) + x(1); };

  verify_zero_bc(fe_space, u0_vector);
  Eigen::VectorXd u_new = reaction_step(fe_space, u0_vector, c_function, 1.0);
  verify_zero_bc(fe_space, u_new);
}

TEST(SemiLagrStep, boundary_conditions) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(4, 1.0);
  auto fe_space =
      std::make_shared<const lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  auto u_function = [](Eigen::VectorXd x) {
    return x(1) * x(0) * (1 - x(1)) * (1 - x(0));
  };
  Eigen::VectorXd u0_vector = lf::fe::NodalProjection(
      *fe_space, lf::mesh::utils::MeshFunctionGlobal(u_function));

  auto v = [](Eigen::Vector2d x) {
    return (Eigen::Vector2d() << -x(1) + 3.0 * x(0) * x(0), x(0)).finished();
  };

  verify_zero_bc(fe_space, u0_vector);
  Eigen::VectorXd u_new = semiLagr_step(fe_space, u0_vector, v, 1.0);
  verify_zero_bc(fe_space, u_new);
}

TEST(solverot, boundary_conditions) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(4, 1.0);
  auto fe_space =
      std::make_shared<const lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  auto u_function = [](Eigen::VectorXd x) {
    return x(1) * x(0) * (1 - x(1)) * (1 - x(0));
  };
  Eigen::VectorXd u0_vector = lf::fe::NodalProjection(
      *fe_space, lf::mesh::utils::MeshFunctionGlobal(u_function));

  Eigen::VectorXd u_new = solverot(fe_space, u0_vector, 10, 1.0);
  verify_zero_bc(fe_space, u_new);
}

TEST(solvetrp, boundary_conditions) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(4, 1.0);
  auto fe_space =
      std::make_shared<const lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  auto u_function = [](Eigen::VectorXd x) {
    return x(1) * x(0) * (1 - x(1)) * (1 - x(0));
  };

  Eigen::VectorXd u0_vector = lf::fe::NodalProjection(
      *fe_space, lf::mesh::utils::MeshFunctionGlobal(u_function));
  Eigen::VectorXd u_new = solvetrp(fe_space, u0_vector, 10, 1.0);
  verify_zero_bc(fe_space, u_new);
}

// The following two test cases test the reaction_step function
// based on an exact solution u(x,t). Even though this exact solution
// is a polynomial of degree four in x, it is still the case that
// the approximate solution to the evolution problem is a good approximation
// of the nodal projection of the exact solution at time T. Since
// the reaction_step relies on a lumped mass matrix, the system of
// equations decouble and for each component ODE the RK-scheme is applied.

// u(x,t) = x0*(1-x0)*x1*(1-x1)*exp(2t)
TEST(ReactionStep, exact_solution_constant_c) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(4, 1.0);
  auto fe_space =
      std::make_shared<const lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  // solution function at initial time t=0
  auto u0_function = [](Eigen::VectorXd x) {
    return x(1) * x(0) * (1 - x(1)) * (1 - x(0));
  };

  // solution function at final time t=1
  auto u1_function = [](Eigen::VectorXd x) {
    return x(1) * x(0) * (1 - x(1)) * (1 - x(0)) * std::exp(2 * 1.0);
  };

  // c function in the ODE:
  auto c_function = [](Eigen::VectorXd /*x*/) { return 2.0; };

  Eigen::VectorXd u0_vector = lf::fe::NodalProjection(
      *fe_space, lf::mesh::utils::MeshFunctionGlobal(u0_function));
  Eigen::VectorXd u1_vector = lf::fe::NodalProjection(
      *fe_space, lf::mesh::utils::MeshFunctionGlobal(u1_function));

  // apply 100 reaction steps with time step 0.01
  for (int i = 0; i < 100; ++i) {
    u0_vector = reaction_step(fe_space, u0_vector, c_function, 0.01);
  }
  EXPECT_NEAR((u0_vector - u1_vector).norm(), 0.0, 1.0E-3);
}

// based on exact solution u(x,t) = x0*(1-x0)*x1*(1-x1)*exp(2t*x0 + t*x1))
TEST(ReactionStep, exact_solution_linear_c) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(4, 1.0);
  auto fe_space =
      std::make_shared<const lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  // solution function at initial time t=0
  auto u0_function = [](Eigen::VectorXd x) {
    return x(1) * x(0) * (1 - x(1)) * (1 - x(0));
  };

  // solution function at final time t=1
  auto u1_function = [](Eigen::VectorXd x) {
    return x(1) * x(0) * (1 - x(1)) * (1 - x(0)) * std::exp(2 * x(0) + x(1));
  };

  // c function in the ODE:
  auto c_function = [](Eigen::VectorXd x) { return 2.0 * x(0) + x(1); };

  Eigen::VectorXd u0_vector = lf::fe::NodalProjection(
      *fe_space, lf::mesh::utils::MeshFunctionGlobal(u0_function));
  Eigen::VectorXd u1_vector = lf::fe::NodalProjection(
      *fe_space, lf::mesh::utils::MeshFunctionGlobal(u1_function));

  // apply 100 reaction steps with time step 0.01
  for (int i = 0; i < 100; ++i) {
    u0_vector = reaction_step(fe_space, u0_vector, c_function, 0.01);
  }
  EXPECT_NEAR((u0_vector - u1_vector).norm(), 0.0, 1.0E-3);
}

// The following few testcases verify that solverot (and solvetrp) satisfy the
// following: If solverot simulates N timesteps of the PDE on [0,T] with initial
// condition u_0 the result should be the same as simulating N/2 timesteps on
// [0,T/2] and then another N/2 timestpes on [0,T/2] with the updated initial
// conditin, since all coefficients are indepentent of time.
TEST(solverot, consistency_1) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(4, 1.0);
  auto fe_space =
      std::make_shared<const lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  auto u0_function = [](Eigen::VectorXd x) {
    return x(1) * x(0) * (1 - x(1)) * (1 - x(0));
  };
  Eigen::VectorXd u0_vector = lf::fe::NodalProjection(
      *fe_space, lf::mesh::utils::MeshFunctionGlobal(u0_function));

  Eigen::VectorXd sol_1 = solverot(fe_space, u0_vector, 2, 1.0);
  Eigen::VectorXd sol_2 =
      solverot(fe_space, solverot(fe_space, u0_vector, 1, 0.5), 1, 0.5);

  EXPECT_NEAR((sol_1 - sol_2).norm(), 0.0, 1.0E-4);
}

TEST(solverot, consistency_2) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(4, 1.0);
  auto fe_space =
      std::make_shared<const lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  auto u0_function = [](Eigen::VectorXd x) {
    return x(1) * x(0) * (1 - x(1)) * (1 - x(0));
  };
  Eigen::VectorXd u0_vector = lf::fe::NodalProjection(
      *fe_space, lf::mesh::utils::MeshFunctionGlobal(u0_function));

  Eigen::VectorXd sol_1 = solverot(fe_space, u0_vector, 10, 1.0);
  Eigen::VectorXd sol_2 =
      solverot(fe_space, solverot(fe_space, u0_vector, 5, 0.5), 5, 0.5);

  EXPECT_NEAR((sol_1 - sol_2).norm(), 0.0, 1.0E-4);
}

TEST(solverot, consistency_3) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(4, 1.0);
  auto fe_space =
      std::make_shared<const lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  auto u0_function = [](Eigen::VectorXd x) {
    return x(1) * x(0) * (1 - x(1)) * (1 - x(0));
  };
  Eigen::VectorXd u0_vector = lf::fe::NodalProjection(
      *fe_space, lf::mesh::utils::MeshFunctionGlobal(u0_function));

  Eigen::VectorXd sol_1 = solverot(fe_space, u0_vector, 10, 1.0);
  Eigen::VectorXd sol_2 =
      solverot(fe_space, solverot(fe_space, u0_vector, 1, 0.1), 9, 0.9);

  EXPECT_NEAR((sol_1 - sol_2).norm(), 0.0, 1.0E-4);
}

TEST(solvetrp, consistency_1) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(4, 1.0);
  auto fe_space =
      std::make_shared<const lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  auto u0_function = [](Eigen::VectorXd x) {
    return x(1) * x(0) * (1 - x(1)) * (1 - x(0));
  };
  Eigen::VectorXd u0_vector = lf::fe::NodalProjection(
      *fe_space, lf::mesh::utils::MeshFunctionGlobal(u0_function));

  Eigen::VectorXd sol_1 = solvetrp(fe_space, u0_vector, 2, 1.0);
  Eigen::VectorXd sol_2 =
      solvetrp(fe_space, solvetrp(fe_space, u0_vector, 1, 0.5), 1, 0.5);

  EXPECT_NEAR((sol_1 - sol_2).norm(), 0.0, 1.0E-4);
}

TEST(solvetrp, consistency_2) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(4, 1.0);
  auto fe_space =
      std::make_shared<const lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  auto u0_function = [](Eigen::VectorXd x) {
    return x(1) * x(0) * (1 - x(1)) * (1 - x(0));
  };
  Eigen::VectorXd u0_vector = lf::fe::NodalProjection(
      *fe_space, lf::mesh::utils::MeshFunctionGlobal(u0_function));

  Eigen::VectorXd sol_1 = solvetrp(fe_space, u0_vector, 10, 1.0);
  Eigen::VectorXd sol_2 =
      solvetrp(fe_space, solvetrp(fe_space, u0_vector, 5, 0.5), 5, 0.5);

  EXPECT_NEAR((sol_1 - sol_2).norm(), 0.0, 1.0E-4);
}

TEST(solvetrp, consistency_3) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(4, 1.0);
  auto fe_space =
      std::make_shared<const lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  auto u0_function = [](Eigen::VectorXd x) {
    return x(1) * x(0) * (1 - x(1)) * (1 - x(0));
  };
  Eigen::VectorXd u0_vector = lf::fe::NodalProjection(
      *fe_space, lf::mesh::utils::MeshFunctionGlobal(u0_function));

  Eigen::VectorXd sol_1 = solvetrp(fe_space, u0_vector, 10, 1.0);
  Eigen::VectorXd sol_2 =
      solvetrp(fe_space, solvetrp(fe_space, u0_vector, 1, 0.1), 9, 0.9);

  EXPECT_NEAR((sol_1 - sol_2).norm(), 0.0, 1.0E-4);
}

// The final set of test cases checks the output of solverot and solvetrp
// against
// a numerical refernce solution.

TEST(solverot, reference_1) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(4, 1.0);
  auto fe_space =
      std::make_shared<const lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  auto u0_function = [](Eigen::VectorXd x) {
    return x(1) * x(0) * (1 - x(1)) * (1 - x(0));
  };
  Eigen::VectorXd u0_vector = lf::fe::NodalProjection(
      *fe_space, lf::mesh::utils::MeshFunctionGlobal(u0_function));

  Eigen::VectorXd u_ref(u0_vector.size());
  u_ref << 0, 0, 0, 0, 0, 0.0159701, 0.0150475, 0, 0, 0.0171946, 0.0170436, 0,
      0, 0, 0, 0;
  Eigen::VectorXd u = solverot(fe_space, u0_vector, 1, 0.1);

  EXPECT_NEAR((u - u_ref).norm(), 0.0, 1.0E-5);
}

TEST(solverot, reference_2) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(4, 1.0);
  auto fe_space =
      std::make_shared<const lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  auto u0_function = [](Eigen::VectorXd x) {
    return x(1) * x(0) * (1 - x(1)) * (1 - x(0));
  };
  Eigen::VectorXd u0_vector = lf::fe::NodalProjection(
      *fe_space, lf::mesh::utils::MeshFunctionGlobal(u0_function));

  Eigen::VectorXd u_ref(u0_vector.size());
  u_ref << 0, 0, 0, 0, 0, 0.00860437, 0.00827551, 0, 0, 0.00910899, 0.00901993,
      0, 0, 0, 0, 0;

  Eigen::VectorXd u = solverot(fe_space, u0_vector, 10, 0.1);

  EXPECT_NEAR((u - u_ref).norm(), 0.0, 1.0E-5);
}

TEST(solvetrp, reference_1) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(4, 1.0);
  auto fe_space =
      std::make_shared<const lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  auto u0_function = [](Eigen::VectorXd x) {
    return x(1) * x(0) * (1 - x(1)) * (1 - x(0));
  };
  Eigen::VectorXd u0_vector = lf::fe::NodalProjection(
      *fe_space, lf::mesh::utils::MeshFunctionGlobal(u0_function));

  Eigen::VectorXd u_ref(u0_vector.size());
  u_ref << 0, 0, 0, 0, 0, 0.00993255, 0.00841276, 0, 0, 0.0105583, 0.00937419,
      0, 0, 0, 0, 0;
  Eigen::VectorXd u = solvetrp(fe_space, u0_vector, 1, 0.1);

  EXPECT_NEAR((u - u_ref).norm(), 0.0, 1.0E-5);
}

TEST(solvetrp, reference_2) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(4, 1.0);
  auto fe_space =
      std::make_shared<const lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  auto u0_function = [](Eigen::VectorXd x) {
    return x(1) * x(0) * (1 - x(1)) * (1 - x(0));
  };
  Eigen::VectorXd u0_vector = lf::fe::NodalProjection(
      *fe_space, lf::mesh::utils::MeshFunctionGlobal(u0_function));

  Eigen::VectorXd u_ref(u0_vector.size());
  u_ref << 0, 0, 0, 0, 0, 0.00670092, 0.00583407, 0, 0, 0.00707033, 0.00636342,
      0, 0, 0, 0, 0;
  Eigen::VectorXd u = solvetrp(fe_space, u0_vector, 10, 0.1);
  EXPECT_NEAR((u - u_ref).norm(), 0.0, 1.0E-5);
}

}  // namespace TranspSemiLagr::test


// Copyright (C) 2013 The Regents of the University of California (Regents).
// 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.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>
#include <vector>

#include "gtest/gtest.h"

#include "theia/math/util.h"
#include "theia/util/random.h"
#include "theia/util/util.h"
#include "theia/matching/feature_correspondence.h"
#include "theia/sfm/triangulation/triangulation.h"
#include "theia/sfm/pose/test_util.h"

namespace theia {
using Eigen::MatrixXd;
using Eigen::Matrix3d;
using Eigen::Quaterniond;
using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::Vector4d;

namespace {

RandomNumberGenerator rng(59);

enum TriangulationType {
  STANDARD = 1,
  DLT = 2,
  MIDPOINT = 3
};

double ReprojectionError(const Matrix3x4d& pose,
                         const Vector4d& world_point,
                         const Vector2d& image_point) {
  const Vector3d reprojected_point = pose * world_point;
  const double sq_reproj_error =
      (reprojected_point.hnormalized() - image_point).squaredNorm();
  return sq_reproj_error;
}

void TestTriangulationBasic(const TriangulationType type,
                            const Vector3d& point_3d,
                            const Quaterniond& rel_rotation,
                            const Vector3d& rel_translation,
                            const double projection_noise,
                            const double max_reprojection_error) {
  Matrix3x4d pose1;
  pose1 <<
      rel_rotation.toRotationMatrix(), rel_translation.normalized();
  const Matrix3x4d pose2 = Matrix3x4d::Identity();

  // Reproject point into both image 2, assume image 1 is identity rotation at
  // the origin.
  Vector2d image_point1 =
      (pose1 * point_3d.homogeneous()).eval().hnormalized();
  Vector2d image_point2 =
      (pose2 * point_3d.homogeneous()).eval().hnormalized();

  // Add projection noise if required.
  if (projection_noise) {
    AddNoiseToProjection(projection_noise, &rng, &image_point1);
    AddNoiseToProjection(projection_noise, &rng, &image_point2);
  }

  // Triangulate with Optimal.
  Vector4d triangulated_point;
  if (type == STANDARD) {
    EXPECT_TRUE(Triangulate(pose1, pose2,
                            image_point1, image_point2,
                            &triangulated_point));
  } else if (type == DLT) {
    EXPECT_TRUE(
        TriangulateDLT(pose1, pose2,
                       image_point1, image_point2,
                       &triangulated_point));
  } else if (type == MIDPOINT) {
    std::vector<Eigen::Vector3d> origins;
    std::vector<Eigen::Vector3d> directions;

    const Matrix3d rotation1 = pose1.block<3, 3>(0, 0);
    origins.emplace_back(-rotation1.transpose() * pose1.col(3));
    directions.emplace_back(
        (rotation1.transpose() * image_point1.homogeneous()).normalized());
    const Matrix3d rotation2 = pose2.block<3, 3>(0, 0);
    origins.emplace_back(-rotation2.transpose() * pose2.col(3));
    directions.emplace_back(
        (rotation2.transpose() * image_point2.homogeneous()).normalized());
    EXPECT_TRUE(
        TriangulateMidpoint(origins, directions, &triangulated_point));
  } else {
    LOG(ERROR) << "Incompatible Triangulation type!";
  }

  // Check the reprojection error.
  EXPECT_LE(
      ReprojectionError(pose1, triangulated_point, image_point1),
      max_reprojection_error);
  EXPECT_LE(
      ReprojectionError(pose2, triangulated_point, image_point2),
      max_reprojection_error);
}

void TestTriangulationManyPoints(const double projection_noise,
                                 const double max_reprojection_error) {
  using Eigen::AngleAxisd;

  static const int num_views = 8;

  // Sets some test rotations and translations.
  static const Quaterniond kRotations[num_views] = {
    Quaterniond(
        AngleAxisd(DegToRad(7.0), Vector3d(0.0, 0.0, 1.0).normalized())),
    Quaterniond(
        AngleAxisd(DegToRad(12.0), Vector3d(0.0, 1.0, 0.0).normalized())),
    Quaterniond(
        AngleAxisd(DegToRad(15.0), Vector3d(1.0, 0.0, 0.0).normalized())),
    Quaterniond(
        AngleAxisd(DegToRad(20.0), Vector3d(1.0, 0.0, 1.0).normalized())),
    Quaterniond(
        AngleAxisd(DegToRad(11.0), Vector3d(0.0, 1.0, 1.0).normalized())),
    Quaterniond(
        AngleAxisd(DegToRad(0.0), Vector3d(1.0, 1.0, 1.0).normalized())),
    Quaterniond(
        AngleAxisd(DegToRad(5.0), Vector3d(0.0, 1.0, 1.0).normalized())),
    Quaterniond(AngleAxisd(DegToRad(0.0), Vector3d(1.0, 1.0, 1.0).normalized()))
  };

  static const Vector3d kTranslations[num_views] = {
    Vector3d(1.0, 1.0, 1.0),
    Vector3d(3.0, 2.0, 13.0),
    Vector3d(4.0, 5.0, 11.0),
    Vector3d(1.0, 2.0, 15.0),
    Vector3d(3.0, 1.5, 91.0),
    Vector3d(1.0, 7.0, 11.0),
    Vector3d(0.0, 0.0, 0.0),  // Tests no translation.
    Vector3d(0.0, 0.0, 0.0)  // Tests no translation and no rotation.
  };

  // Set up model points.
  static const double kTestPoints[][3] = {
    { -1.62, -2.99, 6.12 }, { 4.42, -1.53, 9.83 }, { 1.45, -0.59, 5.29 },
    { 1.89, -1.10, 8.22 }, { -0.21, 2.38, 5.63 }, { 0.61, -0.97, 7.49 },
    { 0.48, 0.70, 8.94 }, { 1.65, -2.56, 8.63 }, { 2.44, -0.20, 7.78 },
    { 2.84, -2.58, 7.35 }, { -1.35, -2.84, 7.33 }, { -0.42, 1.54, 8.86 },
    { 2.56, 1.72, 7.86 }, { 1.75, -1.39, 5.73 }, { 2.08, -3.91, 8.37 },
    { -0.91, 1.36, 9.16 }, { 2.84, 1.54, 8.74 }, { -1.01, 3.02, 8.18 },
    { -3.73, -0.62, 7.81 }, { -2.98, -1.88, 6.23 }, { 2.39, -0.19, 6.47 },
    { -0.63, -1.05, 7.11 }, { -1.76, -0.55, 5.18 }, { -3.19, 3.27, 8.18 },
    { 0.31, -2.77, 7.54 }, { 0.54, -3.77, 9.77 },
  };

  Eigen::Matrix3d calibration;
  calibration <<
      800.0, 0.0, 600.0,
      0.0, 800.0, 400.0,
      0.0, 0.0, 1.0;

  // Set up pose matrices.
  std::vector<Matrix3x4d> poses(num_views);
  for (int i = 0; i < num_views; i++) {
    poses[i] << kRotations[i].toRotationMatrix(), kTranslations[i];
  }

  for (int j = 0; j < THEIA_ARRAYSIZE(kTestPoints); j++) {
    // Reproject model point into the images.
    std::vector<Vector2d> image_points(num_views);
    const Vector3d model_point(kTestPoints[j][0], kTestPoints[j][1],
                               kTestPoints[j][2]);
    for (int i = 0; i < num_views; i++) {
      image_points[i] =
          (poses[i] * model_point.homogeneous()).eval().hnormalized();
    }

    // Add projection noise if required.
    if (projection_noise) {
      for (int i = 0; i < num_views; i++) {
        AddNoiseToProjection(projection_noise, &rng, &image_points[i]);
      }
    }

    Vector4d triangulated_point;
    ASSERT_TRUE(TriangulateNView(poses, image_points, &triangulated_point));

    // Check the reprojection error.
    for (int i = 0; i < num_views; i++) {
      EXPECT_LE(ReprojectionError(poses[i],
                                  triangulated_point, image_points[i]),
                max_reprojection_error);
    }
  }
}

TEST(Triangulation, BasicTest) {
  static const double kProjectionNoise = 0.0;
  static const double kReprojectionTolerance = 1e-12;

  // Set up model points.
  const Vector3d points_3d[2] = { Vector3d(5.0, 20.0, 23.0),
                                  Vector3d(-6.0, 16.0, 33.0) };

  // Set up rotations.
  const Quaterniond kRotation(Eigen::AngleAxisd(0.15, Vector3d(0.0, 1.0, 0.0)));

  // Set up translations.
  const Vector3d kTranslation(-3.0, 1.5, 11.0);

  // Run the test.
  for (int i = 0; i < 2; i++) {
    TestTriangulationBasic(TriangulationType::STANDARD,
                           points_3d[i],
                           kRotation,
                           kTranslation,
                           kProjectionNoise,
                           kReprojectionTolerance);
  }
}

TEST(Triangulation, NoiseTest) {
  static const double kProjectionNoise = 1.0 / 512.0;
  static const double kReprojectionTolerance = 1e-5;

  // Set up model points.
  const Vector3d points_3d[2] = { Vector3d(5.0, 20.0, 23.0),
                                  Vector3d(-6.0, 16.0, 33.0) };

  // Set up rotations.
  const Quaterniond kRotation(Eigen::AngleAxisd(0.15, Vector3d(0.0, 1.0, 0.0)));

  // Set up translations.
  const Vector3d kTranslation(-3.0, 1.5, 11.0);

  // Run the test.
  for (int i = 0; i < 2; i++) {
    TestTriangulationBasic(TriangulationType::STANDARD,
                           points_3d[i],
                           kRotation,
                           kTranslation,
                           kProjectionNoise,
                           kReprojectionTolerance);
  }
}

TEST(TriangulationDLT, BasicTest) {
  static const double kProjectionNoise = 0.0;
  static const double kReprojectionTolerance = 1e-12;

  // Set up model points.
  const Vector3d points_3d[2] = { Vector3d(5.0, 20.0, 23.0),
                                  Vector3d(-6.0, 16.0, 33.0) };

  // Set up rotations.
  const Quaterniond kRotation(Eigen::AngleAxisd(0.15, Vector3d(0.0, 1.0, 0.0)));

  // Set up translations.
  const Vector3d kTranslation(-3.0, 1.5, 11.0);

  // Run the test.
  for (int i = 0; i < 2; i++) {
    TestTriangulationBasic(TriangulationType::DLT,
                           points_3d[i],
                           kRotation,
                           kTranslation,
                           kProjectionNoise,
                           kReprojectionTolerance);
  }
}

TEST(TriangulationDLT, NoiseTest) {
  static const double kProjectionNoise = 1.0 / 512.0;
  static const double kReprojectionTolerance = 1e-5;

  // Set up model points.
  const Vector3d points_3d[2] = { Vector3d(5.0, 20.0, 23.0),
                                  Vector3d(-6.0, 16.0, 33.0) };

  // Set up rotations.
  const Quaterniond kRotation(Eigen::AngleAxisd(0.15, Vector3d(0.0, 1.0, 0.0)));

  // Set up translations.
  const Vector3d kTranslation(-3.0, 1.5, 11.0);

  // Run the test.
  for (int i = 0; i < 2; i++) {
    TestTriangulationBasic(TriangulationType::DLT,
                           points_3d[i],
                           kRotation,
                           kTranslation,
                           kProjectionNoise,
                           kReprojectionTolerance);
  }
}

TEST(TriangulationMidpoint, BasicTest) {
  static const double kProjectionNoise = 0.0;
  static const double kReprojectionTolerance = 1e-12;

  // Set up model points.
  const Vector3d points_3d[2] = { Vector3d(5.0, 20.0, 23.0),
                                  Vector3d(-6.0, 16.0, 33.0) };

  // Set up rotations.
  const Quaterniond kRotation(Eigen::AngleAxisd(0.15, Vector3d(0.0, 1.0, 0.0)));

  // Set up translations.
  const Vector3d kTranslation(-3.0, 1.5, 11.0);

  // Run the test.
  for (int i = 0; i < 2; i++) {
    TestTriangulationBasic(TriangulationType::MIDPOINT,
                           points_3d[i],
                           kRotation,
                           kTranslation,
                           kProjectionNoise,
                           kReprojectionTolerance);
  }
}

TEST(TriangulationMidpoint, NoiseTest) {
  static const double kProjectionNoise = 1.0 / 512.0;
  static const double kReprojectionTolerance = 1e-5;

  // Set up model points.
  const Vector3d points_3d[2] = { Vector3d(5.0, 20.0, 23.0),
                                  Vector3d(-6.0, 16.0, 33.0) };

  // Set up rotations.
  const Quaterniond kRotation(Eigen::AngleAxisd(0.15, Vector3d(0.0, 1.0, 0.0)));

  // Set up translations.
  const Vector3d kTranslation(-3.0, 1.5, 11.0);

  // Run the test.
  for (int i = 0; i < 2; i++) {
    TestTriangulationBasic(TriangulationType::MIDPOINT,
                           points_3d[i],
                           kRotation,
                           kTranslation,
                           kProjectionNoise,
                           kReprojectionTolerance);
  }
}

TEST(TriangulationNView, BasicTest) {
  static const double kProjectionNoise = 0.0;
  static const double kReprojectionTolerance = 1e-12;

  // Run the test.
  TestTriangulationManyPoints(kProjectionNoise, kReprojectionTolerance);
}

TEST(TriangulationNView, NoiseTest) {
  static const double kProjectionNoise = 1.0 / 512.0;
  static const double kReprojectionTolerance = 5e-4;

  // Run the test.
  TestTriangulationManyPoints(kProjectionNoise, kReprojectionTolerance);
}

void TestIsTriangulatedPointInFrontOfCameras(
    const Eigen::Vector3d& point3d,
    const Eigen::Matrix3d& rotation,
    const Eigen::Vector3d& translation,
    const bool expected_outcome) {
  FeatureCorrespondence correspondence;
  correspondence.feature1.point_ = point3d.hnormalized();
  correspondence.feature2.point_ =
      (rotation * point3d + translation).hnormalized();
  const Vector3d position = -rotation.transpose() * translation;
  EXPECT_EQ(IsTriangulatedPointInFrontOfCameras(
      correspondence,
      rotation,
      position),
            expected_outcome);
}

TEST(IsTriangulatedPointInFrontOfCameras, InFront) {
  const Matrix3d rotation = Matrix3d::Identity();
  const Vector3d position(-1, 0, 0);
  const Vector3d point(0, 0, 5);
  TestIsTriangulatedPointInFrontOfCameras(point, rotation, position, true);
}

TEST(IsTriangulatedPointInFrontOfCameras, Behind) {
  const Matrix3d rotation = Matrix3d::Identity();
  const Vector3d position(-1, 0, 0);
  const Vector3d point(0, 0, -5);
  TestIsTriangulatedPointInFrontOfCameras(point, rotation, position, false);
}

TEST(IsTriangulatedPointInFrontOfCameras, OneInFrontOneBehind) {
  const Matrix3d rotation = Matrix3d::Identity();
  const Vector3d position(0, 0, -2);
  const Vector3d point(0, 0, 1);
  TestIsTriangulatedPointInFrontOfCameras(point, rotation, position, false);
}

TEST(SufficientTriangulationAngle, AllSufficient) {
  static const double kMinSufficientAngle = 4.0;
  static const double kAngleBetweenCameras = 5.0;

  // Try varying numbers of cameras observing a 3d point from 2 to 50 cameras.
  for (int i = 2; i < 50; i++) {
    // Set up cameras on a unit circle so that the angles are known. Assume that
    // the triangulated point is at (0, 0, 0).
    std::vector<Vector3d> rays;
    for (int j = 0; j < i; j++) {
      rays.emplace_back(cos(DegToRad(j * kAngleBetweenCameras)),
                        sin(DegToRad(j * kAngleBetweenCameras)),
                        0.0);
    }

    EXPECT_TRUE(SufficientTriangulationAngle(rays, kMinSufficientAngle));
  }
}

TEST(SufficientTriangulationAngle, AllInsufficient) {
  static const double kMinSufficientAngle = 4.0;

  // Try varying numbers of cameras observing a 3d point from 2 to 50 cameras.
  for (int i = 2; i < 50; i++) {
    // Set up cameras on a unit circle so that the angles are known. Assume that
    // the triangulated point is at (0, 0, 0).
    std::vector<Vector3d> rays;
    const double angle = kMinSufficientAngle / static_cast<double>(i + 1e-4);
    for (int j = 0; j < i; j++) {
      rays.emplace_back(cos(DegToRad(j * angle)),
                        sin(DegToRad(j * angle)),
                        0.0);
    }

    EXPECT_FALSE(SufficientTriangulationAngle(rays, kMinSufficientAngle));
  }
}

TEST(SufficientTriangulationAngle, SomeInsufficient) {
  static const double kMinSufficientAngle = 4.0;

  // Set up cameras on a unit circle so that the angles are known. Assume that
  // the triangulated point is at (0, 0, 0).
  std::vector<Vector3d> rays;
  rays.emplace_back(cos(DegToRad(0)), sin(DegToRad(0)), 0.0);
  rays.emplace_back(cos(DegToRad(5.0)), sin(DegToRad(5.0)), 0.0);
  rays.emplace_back(cos(DegToRad(1.0)), sin(DegToRad(1.0)), 0.0);

  // We only need one pair of rays to have a sufficient viewing angle, so this
  // should return true since views 0 and 2 have a viewing angle of 5 degree.
  EXPECT_TRUE(SufficientTriangulationAngle(rays, kMinSufficientAngle));
}

TEST(SufficientTriangulationAngle, TwoInsufficient) {
  static const double kMinSufficientAngle = 4.0;

  // Set up cameras on a unit circle so that the angles are known. Assume that
  // the triangulated point is at (0, 0, 0).
  std::vector<Vector3d> rays;
  rays.emplace_back(cos(DegToRad(0)), sin(DegToRad(0)), 0.0);
  rays.emplace_back(cos(DegToRad(1.0)), sin(DegToRad(1.0)), 0.0);

  // We only need one pair of rays to have a sufficient viewing angle, so this
  // should return true since views 0 and 2 have a viewing angle of 5 degree.
  EXPECT_FALSE(SufficientTriangulationAngle(rays, kMinSufficientAngle));
}

}  // namespace
}  // namespace theia


#include "pinocchio/math/multiprecision.hpp"

#include "pinocchio/parsers/urdf.hpp"

#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/rnea.hpp"

#include <boost/multiprecision/cpp_dec_float.hpp>

#include <iostream>

// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here.
#ifndef PINOCCHIO_MODEL_DIR
  #define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
#endif

int main(int argc, char ** argv)
{
  using namespace pinocchio;

  // You should change here to set up your own URDF file or just pass it as an argument of this example.
  const std::string urdf_filename = (argc<=1) ? PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf") : argv[1];

  // Load the URDF model
  Model model;
  pinocchio::urdf::buildModel(urdf_filename,model);

  // Build a data related to model
  Data data(model);

  // Define Model and Data for multiprecision types
  typedef boost::multiprecision::cpp_dec_float_100 float_100;
  typedef ModelTpl<float_100> ModelMulti;
  typedef DataTpl<float_100> DataMulti;

  ModelMulti model_multi = model.cast<float_100>();
  DataMulti data_multi(model_multi);

  // Sample a random joint configuration as well as random joint velocity and acceleration
  ModelMulti::ConfigVectorType q_multi = randomConfiguration(model_multi);
  ModelMulti::TangentVectorType v_multi = ModelMulti::TangentVectorType::Random(model.nv);
  ModelMulti::TangentVectorType a_multi = ModelMulti::TangentVectorType::Random(model.nv);

  Model::ConfigVectorType q = q_multi.cast<double>();
  Model::TangentVectorType v = v_multi.cast<double>();
  Model::TangentVectorType a = a_multi.cast<double>();

  // Computes the inverse dynamics (aka RNEA)
  rnea(model, data, q, v, a);
  rnea(model_multi , data_multi , q_multi , v_multi , a_multi);

  // Get access to the joint torque with standard or multiprecision arithmetic and print sufficient decimals for both precisions
  std::cout << "Joint torque standard arithmetic:\n" << std::setprecision(std::numeric_limits<float_100>::max_digits10) << data.tau << std::endl;
  std::cout << "Joint torque multiprecision arithmetic:\n" << std::setprecision(std::numeric_limits<float_100>::max_digits10) << data_multi.tau << std::endl;
}


#include "advent.hpp"

#include <fmt/color.h>
#include <fmt/ranges.h>
#include <fstream>
#include <gsl/gsl_util>
#include <iostream>
#include <tuple>
#include <ranges>
#include <scn/scn.h>
#include <vector>

#include <Eigen/Core>

using std::ifstream;
using std::pair;
using std::string;
using std::vector;
using std::ranges::views::iota;

using Eigen::Array;

constexpr i64 energy_level = 9;

auto get_block(auto& m, auto i, auto j) -> std::tuple<i64, i64, i64, i64> {
    auto x = std::max(0L, i-1);
    auto y = std::max(0L, j-1);
    return std::make_tuple(x, y,
            std::min(3L - (i == 0), m.rows() - x),
            std::min(3L - (j == 0), m.cols() - y));
}

auto flash(auto& m, auto& f, auto i, auto j) {
    if (f(i, j) || m(i, j) <= energy_level) { return; }
    f(i, j) = 1;
    auto [x, y, w, h] = get_block(m, i, j);
    m.block(x, y, w, h) += 1;
    for (auto ii = x; ii < x + w; ++ii) {
        for (auto jj = y; jj < y + h; ++jj) {
            flash(m, f, ii, jj);
        }
    }
};

auto day11(int argc, char** argv) -> int
{
    if (argc < 2) {
        fmt::print("Error: no input.");
        return 1;
    }

    ifstream infile(argv[1]); // NOLINT
    string line;
    std::getline(infile, line);
    const i64 ncol = std::ssize(line);
    const i64 nrow = std::count(std::istreambuf_iterator<char>(infile), std::istreambuf_iterator<char>(), '\n');
    infile.seekg(0); // rewind the input stream

    Array<i64, -1, -1> map(nrow + 1, ncol);
    i64 row{0};
    while (std::getline(infile, line)) { // NOLINT
        i64 col{0};
        for (auto c : line) {
            map(row, col++) = static_cast<int>(c - '0');
        }
        ++row;
    }

    decltype(map) flashed = decltype(map)::Zero(map.rows(), map.cols());

    i64 part1{0};
    i64 part2{0};
    const i64 part1_steps{100};

    for (auto s = 1L; ; ++s) {
        flashed.fill(0);
        map += 1;

        for (auto i : iota(0L, map.rows())) {
            for (auto j : iota(0L, map.cols())) {
                flash(map, flashed, i, j);
            }
        }
        map = (map > energy_level).select(0, map);
        auto flashes = (map == 0).count();
        if (s <= part1_steps) {
            part1 += flashes;
        }
        if (flashes == map.size()) {
            part2 = s;
            break;
        }
    }
    fmt::print("part 1: {}\n", part1);
    fmt::print("part 2: {}\n", part2);

    return 0;
}


#include <elle/math.hh>

#include <string>

#include <boost/range/irange.hpp>

#include <elle/range.hh>
#include <elle/test.hh>

static
void
sum()
{
  BOOST_TEST(elle::sum() == 0);
  BOOST_TEST(elle::sum(1) == 1);
  BOOST_TEST(elle::sum(1, 2) == 3);
  BOOST_TEST(elle::sum(1, 2, 3) == 6);
}

static
void
product()
{
  BOOST_TEST(elle::product() == 1);
  BOOST_TEST(elle::product(1) == 1);
  BOOST_TEST(elle::product(1, 2) == 2);
  BOOST_TEST(elle::product(1, 2, 3) == 6);
  BOOST_TEST(elle::product(0, 1, 2, 3) == 0);
}

ELLE_TEST_SUITE()
{
  auto& master = boost::unit_test::framework::master_test_suite();
  master.add(BOOST_TEST_CASE(sum));
  master.add(BOOST_TEST_CASE(product));
}


#include "drake/math/gradient_util.h"

#include <array>
#include <random>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <gtest/gtest.h>

#include "drake/common/test_utilities/eigen_matrix_compare.h"

using Eigen::Matrix;
using Eigen::MatrixXd;

namespace drake {
namespace math {
namespace {

GTEST_TEST(DrakeGradientUtilTest, MatGradMult) {
  const int nq = 34;
  const int A_rows = 8;
  const int A_cols = 6;
  auto dA = MatrixXd::Random(A_rows * A_cols, nq).eval();
  auto b = Matrix<double, A_cols, 1>::Random().eval();
  auto dAb = matGradMult(dA, b).eval();
  auto A = Matrix<double, A_rows, A_cols>::Random().eval();
  auto db = MatrixXd::Zero(b.rows(), nq).eval();
  auto dAb_check = matGradMultMat(A, b, dA, db);

  EXPECT_TRUE(CompareMatrices(dAb, dAb_check, 1e-10,
                              MatrixCompareType::absolute));
}

GTEST_TEST(DrakeGradientUtilTest, SetSubMatrixGradient) {
  const int A_rows = 4;
  const int A_cols = 4;
  const int nq = 34;

  std::array<int, 3> rows{{0, 1, 2}};
  std::array<int, 3> cols{{0, 1, 2}};

  int q_start = 2;
  const int q_subvector_size = 3;
  MatrixXd dA_submatrix =
      MatrixXd::Random(rows.size() * cols.size(), q_subvector_size);

  auto dA = Matrix<double, A_rows * A_cols, Eigen::Dynamic>::Random(
                A_rows * A_cols, nq).eval();
  setSubMatrixGradient<Eigen::Dynamic>(dA, dA_submatrix, rows, cols, A_rows,
                                       q_start, q_subvector_size);

  auto dA_submatrix_back = getSubMatrixGradient<Eigen::Dynamic>(
      dA, rows, cols, A_rows, q_start, q_subvector_size);

  EXPECT_TRUE(CompareMatrices(dA_submatrix_back, dA_submatrix, 1e-10,
                              MatrixCompareType::absolute));
}

}  // namespace
}  // namespace math
}  // namespace drake


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

#ifndef SMOOTH__INTERNAL__BUNDLE_HPP_
#define SMOOTH__INTERNAL__BUNDLE_HPP_

#include <array>

#include <Eigen/Core>

#include "common.hpp"
#include "smooth/internal/utils.hpp"

/**
 * @brief Bundle Lie group
 *
 * Represents the direct product
 *  G1 x G2 x ... x Gk
 * for Lie groups G1 ... Gk.
 */
namespace smooth {

using std::get;

template<typename... GsImpl>
struct BundleImpl
{
  using Scalar = std::common_type_t<typename GsImpl::Scalar...>;

  static_assert(
    (std::is_same_v<Scalar, typename GsImpl::Scalar> && ...),
    "Implementation Scalar types must be the same");

  static constexpr std::array<Eigen::Index, sizeof...(GsImpl)> RepSizes{GsImpl::RepSize...};
  static constexpr std::array<Eigen::Index, sizeof...(GsImpl)> Dofs{GsImpl::Dof...};
  static constexpr std::array<Eigen::Index, sizeof...(GsImpl)> Dims{GsImpl::Dim...};

  static constexpr auto RepSizesPsum = smooth::utils::array_psum(RepSizes);
  static constexpr auto DofsPsum     = smooth::utils::array_psum(Dofs);
  static constexpr auto DimsPsum     = smooth::utils::array_psum(Dims);

  template<std::size_t Idx>
  using PartImpl = std::tuple_element_t<Idx, std::tuple<GsImpl...>>;

  static constexpr Eigen::Index RepSize = RepSizesPsum.back();
  static constexpr Eigen::Index Dof     = DofsPsum.back();
  static constexpr Eigen::Index Dim     = DimsPsum.back();

  SMOOTH_DEFINE_REFS;

  // clang-format off

  static void setIdentity(GRefOut g_out)
  {
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::setIdentity(
        g_out.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum))
      );
    });
  }

  static void setRandom(GRefOut g_out)
  {
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::setRandom(
        g_out.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum))
      );
    });
  }

  static void matrix(GRefIn g_in, MRefOut m_out)
  {
    m_out.setZero();
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::matrix(
        g_in.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum)),
        m_out.template block<get<i>(Dims), get<i>(Dims)>(get<i>(DimsPsum), get<i>(DimsPsum))
      );
    });
  }

  static void composition(GRefIn g_in1, GRefIn g_in2, GRefOut g_out)
  {
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::composition(
        g_in1.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum)),
        g_in2.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum)),
        g_out.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum))
      );
    });
  }

  static void inverse(GRefIn g_in, GRefOut g_out)
  {
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::inverse(
        g_in.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum)),
        g_out.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum))
      );
    });
  }

  static void log(GRefIn g_in, TRefOut a_out)
  {
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::log(
        g_in.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum)),
        a_out.template segment<get<i>(Dofs)>(get<i>(DofsPsum))
      );
    });
  }

  static void Ad(GRefIn g_in, TMapRefOut A_out)
  {
    A_out.setZero();
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::Ad(
        g_in.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum)),
        A_out.template block<get<i>(Dofs), get<i>(Dofs)>(get<i>(DofsPsum), get<i>(DofsPsum))
      );
    });
  }

  static void exp(TRefIn a_in, GRefOut g_out)
  {
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::exp(
        a_in.template segment<get<i>(Dofs)>(get<i>(DofsPsum)),
        g_out.template segment<get<i>(RepSizes)>(get<i>(RepSizesPsum))
      );
    });
  }

  static void hat(TRefIn a_in, MRefOut A_out)
  {
    A_out.setZero();
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::hat(
        a_in.template segment<get<i>(Dofs)>(get<i>(DofsPsum)),
        A_out.template block<get<i>(Dims), get<i>(Dims)>(get<i>(DimsPsum), get<i>(DimsPsum))
      );
    });
  }

  static void vee(MRefIn A_in, TRefOut a_out)
  {
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::vee(
        A_in.template block<get<i>(Dims), get<i>(Dims)>(get<i>(DimsPsum), get<i>(DimsPsum)),
        a_out.template segment<get<i>(Dofs)>(get<i>(DofsPsum))
      );
    });
  }

  static void ad(TRefIn a_in, TMapRefOut A_out) {
    A_out.setZero();
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::ad(
        a_in.template segment<get<i>(Dofs)>(get<i>(DofsPsum)),
        A_out.template block<get<i>(Dofs), get<i>(Dofs)>(get<i>(DofsPsum), get<i>(DofsPsum))
      );
    });
  }

  static void dr_exp(TRefIn a_in, TMapRefOut A_out) {
    A_out.setZero();
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::dr_exp(
        a_in.template segment<get<i>(Dofs)>(get<i>(DofsPsum)),
        A_out.template block<get<i>(Dofs), get<i>(Dofs)>(get<i>(DofsPsum), get<i>(DofsPsum))
      );
    });
  }

  static void dr_expinv(TRefIn a_in, TMapRefOut A_out) {
    A_out.setZero();
    smooth::utils::static_for<sizeof...(GsImpl)>([&](auto i) {
      PartImpl<i>::dr_expinv(
        a_in.template segment<get<i>(Dofs)>(get<i>(DofsPsum)),
        A_out.template block<get<i>(Dofs), get<i>(Dofs)>(get<i>(DofsPsum), get<i>(DofsPsum))
      );
    });
  }

  // clang-format on
};

}  // namespace smooth

#endif  // SMOOTH__INTERNAL__BUNDLE_HPP_


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <gtest/gtest.h>
#include <cmath>
#include <limits>

TEST(MathFunctions, digamma) {
  EXPECT_FLOAT_EQ(boost::math::digamma(0.5), stan::math::digamma(0.5));
  EXPECT_FLOAT_EQ(boost::math::digamma(-1.5), stan::math::digamma(-1.5));
}

TEST(MathFunctions, digamma_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_TRUE(std::isnan(stan::math::digamma(nan)));

  EXPECT_TRUE(std::isnan(stan::math::digamma(-1)));

  EXPECT_TRUE(std::isnormal(stan::math::digamma(1.0E50)));
}


/*
 * $Revision: 558 $ $Date: 2010-11-17 03:20:58 -0800 (Wed, 17 Nov 2010) $
 *
 * Copyright by Astos Solutions GmbH, Germany
 *
 * this file is published under the Astos Solutions Free Public License
 * For details on copyright and terms of use see
 * http://www.astos.de/Astos_Solutions_Free_Public_License.html
 */

#include "StarCatalog.h"
#include "Spectrum.h"
#include "Debug.h"
#include <Eigen/Core>
#include <cmath>
#include <algorithm>

using namespace vesta;
using namespace Eigen;
using namespace std;

/** Create an empty star catalog.
  */
StarCatalog::StarCatalog()
{
}

StarCatalog::~StarCatalog()
{
}

// Convert a Johnson B-V color index to the effective surface temperature. Uses the
// relation from Sekiguchi and Fukugita, "A Study of the B-V Color-Temperature Relation."
// (Astronomical Journal, Aug 2000).
// http://iopscience.iop.org/1538-3881/120/2/1072/pdf/1538-3881_120_2_1072.pdf
//
// bv is the Johnson bv color index
// metallicity is Fe/H
static float BVColorIndexToTeff(float bv, float metallicity = 0.0f, float logG = 0.0f)
{
    const float c0 = 3.939654;
    const float c1 = -0.395361;
    const float c2 = 0.2082113;
    const float c3 = -0.0604097;
    const float f1 = 0.027153;
    const float f2 = 0.005036;
    const float g1 = 0.007367;
    const float h1 = -0.01069;

    float logT = c0 + c1 * bv + c2 * bv * bv + c3 * bv * bv * bv +
                 f1 * metallicity + f2 * metallicity * metallicity +
                 g1 * logG + h1 * bv * logG;

    return pow(10.0f, logT);
}

// Convert the CIE chromaticity coordinates for a black body of the specified
// temperature. The calculation uses a piecewise cubic approximation that is
// valid for temperatures above 1667 K. For cooler temperatures, we simply clamp
// them to 1667 K. This is adequate for our use the function to compute star
// colors as only brown dwarf stars are cooler, and these are so faint that
// they don't need to be represented in VESTA.
static Vector2f planckianLocus(float T)
{
    // Clamp to a valid range
    T = std::max(T, 1667.0f);

    float t = 1000.0f / T;
    float t2 = t * t;
    float t3 = t2 * t;

    float x;
    if (T < 4000.0f)
    {
        x = -0.266162 * t3 - 0.2343580f * t2 + 0.8776956f * t + 0.179910f;
    }
    else
    {
        // Valid from 4000K - 25000K
        x = -3.0258469f * t3 + 2.1070379f * t2 + 0.2226347f * t + 0.24039f;
    }

    float x2 = x * x;
    float x3 = x2 * x;
    float y;
    if (T < 2222)
    {
        // Valid from 1667K - 2222K
        y = -1.1063814f * x3 - 1.3481102 * x2  + 2.18555832f * x - 0.20219683f;
    }
    else if (T < 4000)
    {
        // Valid from 2222K - 4000K
        y = -0.9549976f * x3 - 1.3741859f * x2 + 2.0913702f * x - 0.16748867f;
    }
    else
    {
        // Valid from 4000K - 25000K
        y = 3.0817580f * x3 - 5.8338670f * x2 + 3.75112997f * x - 0.37001483f;
    }

    return Vector2f(x, y);
}

static Vector3f xyToXYZ(const Vector2f& xy)
{
    return Vector3f(xy.x() / xy.y(), 1.0f, (1.0f - xy.x() - xy.y()) / xy.y());
}

static Spectrum linearSrgbStarColor(float bv)
{
    float Teff = BVColorIndexToTeff(bv);
    Vector2f ciexy = planckianLocus(Teff);
    Vector3f cieXYZ = xyToXYZ(ciexy);

    Spectrum srgb = Spectrum::XYZtoLinearSRGB(Spectrum(cieXYZ.x(), cieXYZ.y(), cieXYZ.z()));
    srgb.normalize();
    return srgb;
}

/** Add a new star to the catalog.
  * @param ra the right ascension (in radians)
  * @param dec the declination (in radians)
  * @param vmag the apparent V magnitude in the Johnson photometric system (mean wavelength 540nm)
  * @param bv the value of B-V color index in the Johnson photometric system
  */
void
StarCatalog::addStar(v_uint32 identifier, double ra, double dec, double vmag, double bv)
{
    StarRecord star;
    star.identifier = identifier;
    star.RA = float(ra);
    star.declination = float(dec);
    star.apparentMagnitude = float(vmag);
    star.bvColorIndex = float(bv);

    m_starData.push_back(star);
}

/** Compute the approximate color of a star from it's Johnson B-V color index. The
  * color returned is in the CIE XYZ color space.
  */
Spectrum StarCatalog::StarColor(float bv)
{
    float Teff = BVColorIndexToTeff(bv);
    Vector2f ciexy = planckianLocus(Teff);
    Vector3f cieXYZ = xyToXYZ(ciexy);

    return Spectrum(cieXYZ.x(), cieXYZ.y(), cieXYZ.z());
}

class StarIdPredicate
{
public:
    StarIdPredicate() {}
    bool operator()(const StarCatalog::StarRecord& star0, const StarCatalog::StarRecord& star1) const
    {
        return star0.identifier < star1.identifier;
    }
};

/** Index the star catalog by identifier. This method must be called before star lookups by identifier
  * will work.
  */
void
StarCatalog::buildCatalogIndex()
{
    sort(m_starData.begin(), m_starData.end(), StarIdPredicate());
}

/** Lookup a star by its identifier. Returns null if the star isn't present in the
  * catalog. buildCatalogIndex() must be called once before findStarIdentifier will
  * work.
  */
const StarCatalog::StarRecord*
StarCatalog::findStarIdentifier(v_uint32 id)
{
    StarRecord match;
    match.identifier = id;

    vector<StarRecord>::const_iterator pos = lower_bound(m_starData.begin(), m_starData.end(), match, StarIdPredicate());
    if (pos == m_starData.end())
    {
        return NULL;
    }
    else
    {
        return &(*pos);
    }
}


/*
 * test_MACD.cpp
 *
 *  Created on: 2013-4-11
 *      Author: fasiondog
 */

#ifdef TEST_ALL_IN_ONE
    #include <boost/test/unit_test.hpp>
#else
    #define BOOST_TEST_MODULE test_hikyuu_indicator_suite
    #include <boost/test/unit_test.hpp>
#endif

#include <hikyuu/indicator/crt/MACD.h>
#include <hikyuu/indicator/crt/PRICELIST.h>
#include <hikyuu/indicator/crt/EMA.h>

using namespace hku;

/**
 * @defgroup test_indicator_MACD test_indicator_MACD
 * @ingroup test_hikyuu_indicator_suite
 * @{
 */

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_MACD ) {
    PriceList d;
    for (size_t i = 0; i < 20; ++i) {
        d.push_back(i);
    }

    Indicator ind = PRICELIST(d);
    Indicator macd, bar, diff, dea;
    Indicator ema1, ema2, fast, slow, bmacd;

    /** @arg 源数据为空 */
    macd = MACD(Indicator(), 12, 26, 9);
    BOOST_CHECK(macd.size() == 0);
    BOOST_CHECK(macd.empty() == true);

    /** @arg n1 = n2 = n3 = 1*/
    macd = MACD(ind, 1, 1, 1);
    BOOST_CHECK(macd.getResultNumber() == 3);
    bar = macd.getResult(0);
    diff = macd.getResult(1);
    dea = macd.getResult(2);
    BOOST_CHECK(bar.size() == 20);
    BOOST_CHECK(diff.size() == 20);
    BOOST_CHECK(dea.size() == 20);

    BOOST_CHECK(diff[0] == 0);
    BOOST_CHECK(diff[1] == 0);
    BOOST_CHECK(diff[19] == 0);

    BOOST_CHECK(dea[0] == 0);
    BOOST_CHECK(dea[1] == 0);
    BOOST_CHECK(dea[19] == 0);

    BOOST_CHECK(bar[0] == 0);
    BOOST_CHECK(bar[1] == 0);
    BOOST_CHECK(bar[19] == 0);

    /** @arg n1 = 1 n2 = 2 n3 = 3*/
    macd = MACD(ind, 1, 2, 3);
    BOOST_CHECK(macd.size() == 20);
    BOOST_CHECK(macd.discard() == 0);
    bar = macd.getResult(0);
    diff = macd.getResult(1);
    dea = macd.getResult(2);
    ema1 = EMA(ind, 1);
    ema2 = EMA(ind, 2);
    fast = ema1 - ema2;
    slow = EMA(fast, 3);
    bmacd = fast - slow;
    BOOST_CHECK(bar.size() == 20);
    BOOST_CHECK(diff.size() == 20);
    BOOST_CHECK(dea.size() == 20);

    BOOST_CHECK(diff[0] == fast[0]);
    BOOST_CHECK(diff[1] == fast[1]);
    BOOST_CHECK(diff[19] == fast[19]);

    BOOST_CHECK(dea[0] == slow[0]);
    BOOST_CHECK(std::fabs(dea[1] - slow[1]) < 0.0001);
    BOOST_CHECK(dea[19] == slow[19]);

    BOOST_CHECK(bar[0] == bmacd[0]);
    BOOST_CHECK(bar[1] == bmacd[1]);
    BOOST_CHECK(bar[19] == bmacd[19]);

    /** @arg n1 = 3 n2 = 2 n3 = 1*/
    macd = MACD(ind, 3, 2, 1);
    BOOST_CHECK(macd.size() == 20);
    BOOST_CHECK(macd.discard() == 0);
    bar = macd.getResult(0);
    diff = macd.getResult(1);
    dea = macd.getResult(2);
    ema1 = EMA(ind, 3);
    ema2 = EMA(ind, 2);
    fast = ema1 - ema2;
    slow = EMA(fast, 1);
    bmacd = fast - slow;
    BOOST_CHECK(bar.size() == 20);
    BOOST_CHECK(diff.size() == 20);
    BOOST_CHECK(dea.size() == 20);

    BOOST_CHECK(diff[0] == fast[0]);
    BOOST_CHECK(diff[1] == fast[1]);
    BOOST_CHECK(diff[19] == fast[19]);

    BOOST_CHECK(dea[0] == slow[0]);
    BOOST_CHECK(dea[1] == slow[1]);
    BOOST_CHECK(dea[19] == slow[19]);

    BOOST_CHECK(bar[0] == bmacd[0]);
    BOOST_CHECK(bar[1] == bmacd[1]);
    BOOST_CHECK(bar[19] == bmacd[19]);

    /** @arg n1 = 3 n2 = 5 n3 = 2*/
    macd = MACD(ind, 3, 5, 2);
    BOOST_CHECK(macd.size() == 20);
    BOOST_CHECK(macd.discard() == 0);
    bar = macd.getResult(0);
    diff = macd.getResult(1);
    dea = macd.getResult(2);
    ema1 = EMA(ind, 3);
    ema2 = EMA(ind, 5);
    fast = ema1 - ema2;
    slow = EMA(fast, 2);
    bmacd = fast - slow;
    BOOST_CHECK(bar.size() == 20);
    BOOST_CHECK(diff.size() == 20);
    BOOST_CHECK(dea.size() == 20);

    BOOST_CHECK(diff[0] == fast[0]);
    BOOST_CHECK(diff[1] == fast[1]);
    BOOST_CHECK(diff[19] == fast[19]);

    BOOST_CHECK(dea[0] == slow[0]);
    BOOST_CHECK(dea[1] == slow[1]);
    BOOST_CHECK(dea[19] == slow[19]);

    BOOST_CHECK(bar[0] == bmacd[0]);
    BOOST_CHECK(bar[1] == bmacd[1]);
    BOOST_CHECK(bar[19] == bmacd[19]);

    /** @arg operator() */
    Indicator expect = MACD(ind, 3, 5, 2);
    Indicator tmp = MACD(3, 5, 2);
    Indicator result = tmp(ind);
    BOOST_CHECK(result.size() == expect.size());
    for (size_t i = 0; i < expect.size(); ++i) {
        BOOST_CHECK(result.get(i, 0) == expect.get(i, 0));
        BOOST_CHECK(result.get(i, 1) == expect.get(i, 1));
        BOOST_CHECK(result.get(i, 2) == expect.get(i, 2));
    }
}

/** @} */


#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/eigen.h>
#include <Eigen/Dense>

#include "geometrycentral/surface/halfedge_mesh.h"
#include "geometrycentral/surface/halfedge_factories.h"
#include "geometrycentral/surface/meshio.h"
#include "geometrycentral/surface/heat_method_distance.h"
#include "geometrycentral/surface/fast_marching_method.h"
#include "geometrycentral/surface/vertex_position_geometry.h"
#include "exact_geodesic.h"

using namespace std;
using namespace geometrycentral;
using namespace geometrycentral::surface;
namespace py = pybind11;

// Geometry-central data
std::unique_ptr<HalfedgeMesh> mesh;
std::unique_ptr<VertexPositionGeometry> geometry;

// Loads a mesh from a NumPy array
// source: https://github.com/rubenwiersma/hsn/blob/master/vectorheat/src/main.cpp
std::tuple<std::unique_ptr<HalfedgeMesh>, std::unique_ptr<VertexPositionGeometry>>
loadMesh_np(Eigen::MatrixXd& pos, Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& faces) {

  // Set vertex positions
  std::vector<Vector3> vertexPositions(pos.rows());
  for (size_t i = 0; i < pos.rows(); i++) {
    vertexPositions[i][0] = pos(i, 0);
    vertexPositions[i][1] = pos(i, 1);
    vertexPositions[i][2] = pos(i, 2);
  }

  // Get face list
  std::vector<std::vector<size_t>> faceIndices(faces.rows());
  for (size_t i = 0; i < faces.rows(); i++) {
    faceIndices[i] = {faces(i, 0), faces(i, 1), faces(i, 2)};
  }

  return makeHalfedgeAndGeometry(faceIndices, vertexPositions);
}

// compute geodesic distance using the heat method
Eigen::MatrixXd get_heat_geodesics(Eigen::MatrixXd& pos, Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& faces){
    // Load mesh
    std::tie(mesh, geometry) = loadMesh_np(pos, faces);
    int nverts = mesh->nVertices();

    // Create the Heat Method solver
    HeatMethodDistanceSolver heatSolver(*geometry);

    // create output matrix
    Eigen::MatrixXd geodesic_matrix(nverts, nverts);

    // compute geodesic distances
    int i = 0;
    for (Vertex v : mesh->vertices()) {
        VertexData<double> distToSource = heatSolver.computeDistance(v);
        for (int j = 0; j < nverts; j++){
            geodesic_matrix(i, j) = distToSource[j];
        }
        i++;
    }

    return geodesic_matrix;
}

// compute geodesic distance using the fast marching method
Eigen::MatrixXd get_fmm_geodesics(Eigen::MatrixXd& pos, Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& faces){
    // Load mesh
    std::tie(mesh, geometry) = loadMesh_np(pos, faces);
    int nverts = mesh->nVertices();

    // create output matrix
    Eigen::MatrixXd geodesic_matrix(nverts, nverts);
    double initial_dist = 0.;

    // compute geodesic distances
    int i = 0;
    for (Vertex v : mesh->vertices()) {
        std::vector<std::pair<Vertex, double>> input{std::make_pair(v, initial_dist)};
        VertexData<double> distToSource = FMMDistance(*geometry, input);
        for (int j = 0; j < nverts; j++)
            geodesic_matrix(i, j) = distToSource[j];

        i++;
    }

    return geodesic_matrix;
}

// compute geodesic distance using the Exact geodesic algorithm by Mitchell, Mount and Papadimitriou in 1987
Eigen::MatrixXd get_exact_geodesics(Eigen::MatrixXd& pos, Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>& faces){
    // utils vars
    int nverts = pos.rows();
    Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic> FS, FT;
    Eigen::MatrixXi target(nverts, 1);
    for (int i = 0; i < nverts; i++)
        target(i, 0) = i;

    // create output matrix
    Eigen::MatrixXd geodesic_matrix(nverts, nverts);

    // compute geodesic distances
    for (int i = 0; i < nverts; i++){
        Eigen::MatrixXi source(1, 1);
        source(0, 0) = i;
        Eigen::MatrixXd distToSource(nverts, 1);
        igl::exact_geodesic(pos, faces, source, FS, target, FT, distToSource);

        for (int j = 0; j < nverts; j++)
            geodesic_matrix(i, j) = distToSource(j);
    }

    return geodesic_matrix;
}

PYBIND11_MODULE(tridesic, m) {
    m.def("get_heat_geodesics", &get_heat_geodesics, py::return_value_policy::copy,
          "Compute the geodesic matrix using the heat method");
    m.def("get_fmm_geodesics", &get_fmm_geodesics, py::return_value_policy::copy,
          "Compute the geodesic matrix using the fast marching method");
    m.def("get_exact_geodesics", &get_exact_geodesics, py::return_value_policy::copy,
          "Compute the geodesic matrix using the Exact geodesic algorithm by Mitchell et al.");
}


#include <SBGATPolyhedronGravityModel.hpp>
#include <SBGATPolyhedronGravityModelUQ.hpp>

#include <vtkCleanPolyData.h>
#include <vtkOBJReader.h>

#include <json.hpp>
#include <boost/progress.hpp>

int main(){

	std::ifstream i("input_file.json");
	nlohmann::json input_data;
	i >> input_data;

	std::string PATH_SHAPE = input_data["PATH_SHAPE"];
	double CORRELATION_DISTANCE =  input_data["CORRELATION_DISTANCE"];

	double ERROR_STANDARD_DEV  = input_data["ERROR_STANDARD_DEV"];
	double DENSITY  = input_data["DENSITY"];
	double STEP_SIZE  = input_data["STEP_SIZE"];

	bool UNIT_IN_METERS  = input_data["UNIT_IN_METERS"];
	bool HOLD_MASS_CONSTANT  = input_data["HOLD_MASS_CONSTANT"];

	int PROJECTION_AXIS = input_data["PROJECTION_AXIS"];
	int N_MONTE_CARLO = input_data["N_MONTE_CARLO"];

	std::string OUTPUT_DIR = input_data["OUTPUT_DIR"];

	std::cout << "- Path to shape: " << PATH_SHAPE << std::endl;
	std::cout << "- Standard deviation on point coordinates (m) : " << ERROR_STANDARD_DEV << std::endl;
	std::cout << "- Correlation distance (m) : " << CORRELATION_DISTANCE << std::endl;
	std::cout << "- Density (kg/m^3) : " << DENSITY << std::endl;
	std::cout << "- Monte-Carlo draws : " << N_MONTE_CARLO << std::endl;
	std::cout << "- Step size : " << STEP_SIZE << std::endl;
	std::cout << "- Projection axis : " << PROJECTION_AXIS << std::endl;

	// Reading
	vtkSmartPointer<vtkOBJReader> reader = vtkSmartPointer<vtkOBJReader>::New();
	reader -> SetFileName(PATH_SHAPE.c_str());
	reader -> Update();

	// An instance of SBGATPolyhedronGravityModel is created to evaluate the PGM of
	// the considered polytdata
	vtkSmartPointer<SBGATPolyhedronGravityModel> pgm_filter = vtkSmartPointer<SBGATPolyhedronGravityModel>::New();
	pgm_filter -> SetInputConnection(reader -> GetOutputPort());
	pgm_filter -> SetDensity(DENSITY);

	if(UNIT_IN_METERS){
		pgm_filter -> SetScaleMeters();
	} else{
		pgm_filter -> SetScaleKiloMeters();
	}
	pgm_filter -> Update();

	// An instance of SBGATPolyhedronGravityModelUQ is created to perform
	// uncertainty quantification from the PGM associated to the shape
	SBGATPolyhedronGravityModelUQ pgm_uq;
	pgm_uq.SetModel(pgm_filter);
	pgm_uq.PrecomputeMassPropertiesPartials();

	// Populate the shape vertices covariance
	pgm_uq.ComputeVerticesCovarianceGlobal(ERROR_STANDARD_DEV,CORRELATION_DISTANCE);
	// Regularizing the covariance
	int regularized_eigen_values = pgm_uq.RegularizeCovariance();

	std::cout << regularized_eigen_values << " eigenvalues were regularized\n";

	arma::mat C_CC = pgm_uq.GetCovarianceSquareRoot();
	arma::mat P_CC = pgm_uq.GetVerticesCovariance();
	std::cout << "Maximum absolute error in covariance square root: " << arma::abs(P_CC - C_CC * C_CC.t()).max() << std::endl;

	std::cout << "Saving shape covariance ...\n";
	P_CC.save(OUTPUT_DIR + "full_covariance.txt",arma::raw_ascii);

	// Saving baseline slices
	pgm_uq.TakeAndSaveSlice(0,OUTPUT_DIR + "baseline_slice_x.txt",1e-6);
	pgm_uq.TakeAndSaveSlice(1,OUTPUT_DIR + "baseline_slice_y.txt",1e-6);
	pgm_uq.TakeAndSaveSlice(2,OUTPUT_DIR + "baseline_slice_z.txt",1e-6);

	// First, create the grid from the bounding box
	std::vector<arma::vec::fixed<3> > grid;
	double xmin,xmax,ymin,ymax,zmin,zmax;
	pgm_filter -> GetBoundingBox(xmin,xmax,ymin,ymax,zmin,zmax);

	// Inflate
	xmin *= 2.5;
	xmax *= 2.5;
	ymin *= 2.5;
	ymax *= 2.5;
	zmin *= 2.5;
	zmax *= 2.5;

	double min_dim = std::min(xmin,std::min(ymin,zmin));
	double max_dim = std::max(xmax,std::max(ymax,zmax));

	xmax = max_dim;
	ymax = max_dim;
	zmax = max_dim;

	xmin = min_dim;
	ymin = min_dim;
	zmin = min_dim;

	// Define grid indices
	int i_max,j_max;

	if (PROJECTION_AXIS == 0){
		i_max = 1./STEP_SIZE * (ymax - ymin);
		j_max = 1./STEP_SIZE * (zmax - zmin);

	}
	else if (PROJECTION_AXIS == 1){

		i_max = 1./STEP_SIZE * (xmax - xmin);
		j_max = 1./STEP_SIZE * (zmax - zmin);

	}
	else if (PROJECTION_AXIS == 2){

		i_max = 1./STEP_SIZE * (xmax - xmin);
		j_max = 1./STEP_SIZE * (ymax - ymin);

	}
	else{
		throw(std::runtime_error("PROJECTION_AXIS has to be either 0, 1 or 2. It can't be equal to " + std::to_string(PROJECTION_AXIS)));
	}

	// Create the grid containers
	std::vector<std::vector<int> > indices;
	arma::mat trace_sqrt_cov(i_max,j_max);
	arma::mat reference_acceleration(i_max,j_max);
	arma::mat uncertainty_over_reference_acc_percentage(i_max,j_max);
	arma::mat inside_outside(i_max,j_max);

	// Construct the grid (constant, uniform step size in both directions of space)
	for (int i = 0; i < i_max; ++i){
		for (int j = 0; j < j_max; ++j){

			arma::vec::fixed<3> point;
			if (PROJECTION_AXIS == 0){

				double y = ymin + i * STEP_SIZE;
				double z = zmin + j * STEP_SIZE;

				point(0) = 0;
				point(1) = y;
				point(2) = z;
			}
			else if (PROJECTION_AXIS == 1){

				double x = xmin + i * STEP_SIZE;
				double z = zmin + j * STEP_SIZE;

				point(0) = x;
				point(1) = 0;
				point(2) = z;
			}
			else{

				double x = xmin + i * STEP_SIZE;
				double y = ymin + j * STEP_SIZE;

				point(0) = x;
				point(1) = y;
				point(2) = 0;
			}

			grid.push_back(point);
			indices.push_back(std::vector<int>({i,j}));

			if (pgm_filter -> Contains(point)){
				inside_outside(i,j) = 1;
			}
			else{
				inside_outside(i,j) = 0;
			}
		}
	}
	std::cout << "- Grid size: " << grid.size() << std::endl;

	// Saving the grid to a file
	arma::mat grid_arma(3,grid.size());
	for (unsigned int p = 0; p < grid.size(); ++p){
		grid_arma.col(p) = grid[p];
	}
	grid_arma.save(OUTPUT_DIR + "grid.txt",arma::raw_ascii);

	// Running a Monte Carlo on a subset of the grid
	std::vector<std::vector<arma::vec::fixed<3> > > all_accelerations;
	std::vector<std::vector<double > > all_potentials;
	std::vector<arma::vec> deviations;
	std::vector<double> densities;

	arma::vec::fixed<3> e0 = {1,0,0};
	arma::vec::fixed<3> e1 = {0,1,0};
	arma::vec::fixed<3> e2 = {0,0,1};
	arma::vec::fixed<3> e3 = arma::normalise(arma::vec({1,1,0}));
	arma::vec::fixed<3> e4 = arma::normalise(arma::vec({0,1,1}));
	arma::vec::fixed<3> e5 = arma::normalise(arma::vec({1,0,1}));
	arma::vec::fixed<3> e6 = arma::normalise(arma::vec({-1,1,0}));
	arma::vec::fixed<3> e7 = arma::normalise(arma::vec({0,-1,1}));
	arma::vec::fixed<3> e8 = arma::normalise(arma::vec({1,0,-1}));

	std::vector<arma::vec::fixed<3> > all_positions;
	std::vector<double> distances = {200,300,400,500,600};
	for (auto dist : distances){
		all_positions.push_back(dist * e0);
		all_positions.push_back(dist * e1);
		all_positions.push_back(dist * e2);
		all_positions.push_back(dist * e3);
		all_positions.push_back(dist * e4);
		all_positions.push_back(dist * e5);
		all_positions.push_back(dist * e6);
		all_positions.push_back(dist * e7);
		all_positions.push_back(dist * e8);
		all_positions.push_back(- dist * e0);
		all_positions.push_back(- dist * e1);
		all_positions.push_back(- dist * e2);
		all_positions.push_back(- dist * e3);
		all_positions.push_back(- dist * e4);
		all_positions.push_back(- dist * e5);
		all_positions.push_back(- dist * e6);
		all_positions.push_back(- dist * e7);
		all_positions.push_back(- dist * e8);
	}

	std::cout << "Running MC ... ";

	auto start = std::chrono::system_clock::now();
	SBGATPolyhedronGravityModelUQ::RunMCUQAccelerationInertial(PATH_SHAPE,DENSITY,
		UNIT_IN_METERS,
		HOLD_MASS_CONSTANT,
		C_CC,
		N_MONTE_CARLO,
		all_positions,
		OUTPUT_DIR,
		std::min(30,N_MONTE_CARLO),
		deviations,
		densities,
		all_accelerations);
	auto end = std::chrono::system_clock::now();

	std::chrono::duration<double> elapsed_seconds = end-start;

	std::cout << "Done running MC in " << elapsed_seconds.count() << " s\n";

// Computing MC Dispersions
	arma::vec KL_divergence_analytical_vs_mc(all_positions.size());
	arma::vec abs_value_cov_difference_analytical_vs_mc(all_positions.size());
	arma::vec rel_value_cov_difference_analytical_vs_mc(all_positions.size());
	arma::mat all_positions_arma(3,all_positions.size());

#pragma omp parallel for
	for (int e = 0; e < all_positions.size(); ++e){

		arma::mat accelerations_mc(3,N_MONTE_CARLO);

		for (int sample = 0; sample < N_MONTE_CARLO; ++sample){
			accelerations_mc.col(sample) = all_accelerations[sample][e];
		}

		arma::mat mc_covariances_acc = arma::cov(accelerations_mc.t());

		arma::vec mc_mean_acc = arma::mean(accelerations_mc,1);
		arma::vec reference_acc = pgm_filter -> GetAcceleration(all_positions[e]);

		arma::mat cov_analytical = pgm_uq.GetCovarianceAcceleration(all_positions[e],HOLD_MASS_CONSTANT);
		KL_divergence_analytical_vs_mc(e) = SBGATFilterUQ::KLDivergence(reference_acc,mc_mean_acc,	cov_analytical,mc_covariances_acc);

		abs_value_cov_difference_analytical_vs_mc(e) = arma::abs(arma::vectorise(cov_analytical - mc_covariances_acc)).max();
		rel_value_cov_difference_analytical_vs_mc(e) = arma::norm(cov_analytical - mc_covariances_acc)/arma::trace(mc_covariances_acc) * 100;
		all_positions_arma.col(e) = all_positions[e];
	}

	std::cout << "- Evaluating analytical uncertainties over grid ...\n";

	start = std::chrono::system_clock::now();

	// For every point in the grid, evaluate the analytical acceleration covariance and
	// run a monte carlo to get a statistical covariance to compare against
	boost::progress_display progress(grid.size());

	#pragma omp parallel for
	for (int p = 0; p < grid.size(); ++p){

		int i = indices[p][0];
		int j = indices[p][1];

		const arma::vec::fixed<3> & grid_point = grid[p];

		arma::vec::fixed<3> reference_acceleration_vector = pgm_filter -> GetAcceleration(grid_point);
		arma::mat::fixed<3,3> covariance_acceleration_analytical = pgm_uq.GetCovarianceAcceleration(grid_point,HOLD_MASS_CONSTANT);

		reference_acceleration(i,j) = arma::norm(reference_acceleration_vector);
		trace_sqrt_cov(i,j) = std::sqrt(arma::trace(covariance_acceleration_analytical)) ;
		uncertainty_over_reference_acc_percentage(i,j) = trace_sqrt_cov(i,j) / reference_acceleration(i,j) * 100;

		++progress;
	}

	end = std::chrono::system_clock::now();
	elapsed_seconds = end-start;
	std::cout << "\n-- Done evaluating over grid in " << elapsed_seconds.count() << " s\n";

	trace_sqrt_cov.save(OUTPUT_DIR + "trace_sqrt_cov.txt",arma::raw_ascii);
	reference_acceleration.save(OUTPUT_DIR + "reference_acceleration.txt",arma::raw_ascii);
	inside_outside.save(OUTPUT_DIR + "inside_outside.txt",arma::raw_ascii);
	uncertainty_over_reference_acc_percentage.save(OUTPUT_DIR + "uncertainty_over_reference_acc_percentage.txt",arma::raw_ascii);

	abs_value_cov_difference_analytical_vs_mc.save(OUTPUT_DIR + "abs_value_cov_difference_analytical_vs_mc.txt",arma::raw_ascii);
	rel_value_cov_difference_analytical_vs_mc.save(OUTPUT_DIR + "rel_value_cov_difference_analytical_vs_mc.txt",arma::raw_ascii);
	KL_divergence_analytical_vs_mc.save(OUTPUT_DIR + "KL_divergence_analytical_vs_mc.txt",arma::raw_ascii);
	all_positions_arma.save(OUTPUT_DIR + "all_positions_arma.txt",arma::raw_ascii);

	return 0;
}