/* ************************************************************************* */
TEST( StereoFactor, Jacobian ) {
  // Create the factor with a measurement that is 3 pixels off in x
  StereoPoint2 measurement(323, 318-50, 241);
  TestStereoFactor factor(measurement, model, X(1), L(1), K);

  // Set the linearization point
  Pose3 pose(Rot3(), Point3(0.0, 0.0, -6.25));
  Point3 point(0.0, 0.0, 0.0);

  // Use the factor to calculate the Jacobians
  Matrix H1Actual, H2Actual;
  factor.evaluateError(pose, point, H1Actual, H2Actual);

  // The expected Jacobians
  Matrix H1Expected = Matrix_(3, 6, 0.0,  -625.0, 0.0, -100.0,    0.0,  0.0,
                                    0.0,  -625.0, 0.0, -100.0,    0.0, -8.0,
                                    625.0,   0.0, 0.0,    0.0, -100.0,  0.0);
  Matrix H2Expected = Matrix_(3, 3, 100.0,   0.0, 0.0,
                                    100.0,   0.0, 8.0,
                                    0.0,   100.0, 0.0);

  // Verify the Jacobians are correct
  CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
  CHECK(assert_equal(H2Expected, H2Actual, 1e-3));
}

/* ************************************************************************* */
TEST( ISAM, iSAM_smoother )
{
  Ordering ordering;
  for (int t = 1; t <= 7; t++) ordering += X(t);

  // Create smoother with 7 nodes
  GaussianFactorGraph smoother = createSmoother(7);

  // run iSAM for every factor
  GaussianISAM actual;
  for(boost::shared_ptr<GaussianFactor> factor: smoother) {
    GaussianFactorGraph factorGraph;
    factorGraph.push_back(factor);
    actual.update(factorGraph);
  }

  // Create expected Bayes Tree by solving smoother with "natural" ordering
  GaussianBayesTree expected = *smoother.eliminateMultifrontal(ordering);

  // Verify sigmas in the bayes tree
  for(const GaussianBayesTree::sharedClique& clique: expected.nodes() | br::map_values) {
    GaussianConditional::shared_ptr conditional = clique->conditional();
    EXPECT(!conditional->get_model());
  }

  // Check whether BayesTree is correct
  EXPECT(assert_equal(GaussianFactorGraph(expected).augmentedHessian(), GaussianFactorGraph(actual).augmentedHessian()));

  // obtain solution
  VectorValues e; // expected solution
  for (int t = 1; t <= 7; t++) e.insert(X(t), Vector::Zero(2));
  VectorValues optimized = actual.optimize(); // actual solution
  EXPECT(assert_equal(e, optimized));
}

/* ************************************************************************* */
TEST(NonlinearOptimizer, NullFactor) {

  example::Graph fg = example::createReallyNonlinearFactorGraph();

  // Add null factor
  fg.push_back(example::Graph::sharedFactor());

  // test error at minimum
  Point2 xstar(0,0);
  Values cstar;
  cstar.insert(X(1), xstar);
  DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);

  // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
  Point2 x0(3,3);
  Values c0;
  c0.insert(X(1), x0);
  DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);

  // optimize parameters
  Ordering ord;
  ord.push_back(X(1));

  // Gauss-Newton
  Values actual1 = GaussNewtonOptimizer(fg, c0, ord).optimize();
  DOUBLES_EQUAL(0,fg.error(actual1),tol);

  // Levenberg-Marquardt
  Values actual2 = LevenbergMarquardtOptimizer(fg, c0, ord).optimize();
  DOUBLES_EQUAL(0,fg.error(actual2),tol);

  // Dogleg
  Values actual3 = DoglegOptimizer(fg, c0, ord).optimize();
  DOUBLES_EQUAL(0,fg.error(actual3),tol);
}

/*******************************************************************************
 * Camera: f = 1, Image: 100x100, center: 50, 50.0
 * Pose (ground truth): (Xw, -Yw, -Zw, [0,0,2.0]')
 * Known landmarks:
 *    3D Points: (10,10,0) (-10,10,0) (-10,-10,0) (10,-10,0)
 * Perfect measurements:
 *    2D Point:  (55,45)   (45,45)    (45,55)     (55,55)
 *******************************************************************************/
int main(int argc, char* argv[]) {
  /* read camera intrinsic parameters */
  Cal3_S2::shared_ptr calib(new Cal3_S2(1, 1, 0, 50, 50));

  /* 1. create graph */
  NonlinearFactorGraph graph;

  /* 2. add factors to the graph */
  // add measurement factors
  SharedDiagonal measurementNoise = Diagonal::Sigmas((Vector(2) << 0.5, 0.5));
  boost::shared_ptr<ResectioningFactor> factor;
  graph.push_back(
      boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(55, 45), Point3(10, 10, 0)));
  graph.push_back(
      boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(45, 45), Point3(-10, 10, 0)));
  graph.push_back(
      boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(45, 55), Point3(-10, -10, 0)));
  graph.push_back(
      boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(55, 55), Point3(10, -10, 0)));

  /* 3. Create an initial estimate for the camera pose */
  Values initial;
  initial.insert(X(1),
      Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 2)));

  /* 4. Optimize the graph using Levenberg-Marquardt*/
  Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
  result.print("Final result:\n");

  return 0;
}

/* ************************************************************************* */
TEST( ProjectionFactorPPPC, JacobianWithTransform ) {
  // Create the factor with a measurement that is 3 pixels off in x
  Point2 measurement(323.0, 240.0);
  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
  TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));

  // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
  Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
  Point3 point(0.0, 0.0, 0.0);

  // Use the factor to calculate the Jacobians
  Matrix H1Actual, H2Actual, H3Actual, H4Actual;
  factor.evaluateError(pose, body_P_sensor, point, *K1, H1Actual, H2Actual, H3Actual, H4Actual);

  // The expected Jacobians
  Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376).finished();
  Matrix H3Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376).finished();

  // Verify the Jacobians are correct
  CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
  CHECK(assert_equal(H3Expected, H3Actual, 1e-3));

  // Verify H2 and H4 with numerical derivatives
  Matrix H2Expected = numericalDerivative11<Vector, Pose3>(
      boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point,
          *K1, boost::none, boost::none, boost::none, boost::none), body_P_sensor);

  Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>(
      boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point,
          _1, boost::none, boost::none, boost::none, boost::none), *K1);

  CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
  CHECK(assert_equal(H4Expected, H4Actual, 1e-5));

}

/* ************************************************************************* */
TEST( simulated2DOriented, constructor )
{
  Pose2 measurement(0.2, 0.3, 0.1);
  SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1., 1., 1.));
  simulated2DOriented::Odometry factor(measurement, model, X(1), X(2));

  simulated2DOriented::Values config;
  config.insert(X(1), Pose2(1., 0., 0.2));
  config.insert(X(2), Pose2(2., 0., 0.1));
  boost::shared_ptr<GaussianFactor> lf = factor.linearize(config);
}

/* ************************************************************************* */
TEST( GaussianBayesTree, balanced_smoother_shortcuts )
{
  // Create smoother with 7 nodes
  GaussianFactorGraph smoother = createSmoother(7);

  // Create the Bayes tree
  Ordering ordering;
  ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
  GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);

  // Check the conditional P(Root|Root)
  GaussianBayesNet empty;
  GaussianBayesTree::sharedClique R = bayesTree.roots().front();
  GaussianBayesNet actual1 = R->shortcut(R);
  EXPECT(assert_equal(empty,actual1,tol));

  // Check the conditional P(C2|Root)
  GaussianBayesTree::sharedClique C2 = bayesTree[X(3)];
  GaussianBayesNet actual2 = C2->shortcut(R);
  EXPECT(assert_equal(empty,actual2,tol));

  // Check the conditional P(C3|Root), which should be equal to P(x2|x4)
  /** TODO: Note for multifrontal conditional:
   * p_x2_x4 is now an element conditional of the multifrontal conditional bayesTree[ordering[X(2)]]->conditional()
   * We don't know yet how to take it out.
   */
//  GaussianConditional::shared_ptr p_x2_x4 = bayesTree[ordering[X(2)]]->conditional();
//  p_x2_x4->print("Conditional p_x2_x4: ");
//  GaussianBayesNet expected3(p_x2_x4);
//  GaussianISAM::sharedClique C3 = isamTree[ordering[X(1)]];
//  GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R);
//  EXPECT(assert_equal(expected3,actual3,tol));
}

/* ************************************************************************* */
TEST( GaussianFactor, getDim )
{
	const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
	// get a factor
  Ordering ordering; ordering += kx1,kx2,kl1;
  GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering);
	GaussianFactor::shared_ptr factor = fg[0];

	// get the size of a variable
	size_t actual = factor->getDim(factor->find(ordering[kx1]));

	// verify
	size_t expected = 2;
	EXPECT_LONGS_EQUAL(expected, actual);
}

/* ************************************************************************* */
TEST( SymbolicFactorGraph, symbolicFactorGraph )
{
  Ordering o; o += X(1),L(1),X(2);
	// construct expected symbolic graph
	SymbolicFactorGraph expected;
	expected.push_factor(o[X(1)]);
	expected.push_factor(o[X(1)],o[X(2)]);
	expected.push_factor(o[X(1)],o[L(1)]);
	expected.push_factor(o[X(2)],o[L(1)]);

	// construct it from the factor graph
	GaussianFactorGraph factorGraph = example::createGaussianFactorGraph(o);
	SymbolicFactorGraph actual(factorGraph);

	CHECK(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(DoglegOptimizer, Iterate) {
  // really non-linear factor graph
  NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();

  // config far from minimum
  Point2 x0(3,0);
  Values config;
  config.insert(X(1), x0);

  double Delta = 1.0;
  for(size_t it=0; it<10; ++it) {
    GaussianBayesNet gbn = *fg.linearize(config)->eliminateSequential();
    // Iterate assumes that linear error = nonlinear error at the linearization point, and this should be true
    double nonlinearError = fg.error(config);
    double linearError = GaussianFactorGraph(gbn).error(config.zeroVectors());
    DOUBLES_EQUAL(nonlinearError, linearError, 1e-5);
//    cout << "it " << it << ", Delta = " << Delta << ", error = " << fg->error(*config) << endl;
    VectorValues dx_u = gbn.optimizeGradientSearch();
    VectorValues dx_n = gbn.optimize();
    DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, dx_u, dx_n, gbn, fg, config, fg.error(config));
    Delta = result.Delta;
    EXPECT(result.f_error < fg.error(config)); // Check that error decreases
    Values newConfig(config.retract(result.dx_d));
    config = newConfig;
    DOUBLES_EQUAL(fg.error(config), result.f_error, 1e-5); // Check that error is correctly filled in
  }
}

/* ************************************************************************* */
TEST( GaussianFactor, matrix )
{
	const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
	// create a small linear factor graph
  Ordering ordering; ordering += kx1,kx2,kl1;
  GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering);

	// get the factor kf2 from the factor graph
	//GaussianFactor::shared_ptr lf = fg[1]; // NOTE: using the older version
	Vector b2 = Vector_(2, 0.2, -0.1);
	Matrix I = eye(2);
  // render with a given ordering
  Ordering ord;
  ord += kx1,kx2;
	JacobianFactor::shared_ptr lf(new JacobianFactor(ord[kx1], -I, ord[kx2], I, b2, sigma0_1));

	// Test whitened version
	Matrix A_act1; Vector b_act1;
	boost::tie(A_act1,b_act1) = lf->matrix(true);

	Matrix A1 = Matrix_(2,4,
			-10.0,  0.0, 10.0,  0.0,
			000.0,-10.0,  0.0, 10.0 );
	Vector b1 = Vector_(2, 2.0, -1.0);

	EQUALITY(A_act1,A1);
	EQUALITY(b_act1,b1);

	// Test unwhitened version
	Matrix A_act2; Vector b_act2;
	boost::tie(A_act2,b_act2) = lf->matrix(false);


	Matrix A2 = Matrix_(2,4,
			-1.0,  0.0, 1.0,  0.0,
			000.0,-1.0,  0.0, 1.0 );
	//Vector b2 = Vector_(2, 2.0, -1.0);

	EQUALITY(A_act2,A2);
	EQUALITY(b_act2,b2);

	// Ensure that whitening is consistent
	boost::shared_ptr<noiseModel::Gaussian> model = lf->get_model();
	model->WhitenSystem(A_act2, b_act2);
	EQUALITY(A_act1, A_act2);
	EQUALITY(b_act1, b_act2);
}

/* ************************************************************************* */
TEST( GaussianFactor, linearFactor )
{
	const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
  Ordering ordering; ordering += kx1,kx2,kl1;

  Matrix I = eye(2);
	Vector b = Vector_(2, 2.0, -1.0);
	JacobianFactor expected(ordering[kx1], -10*I,ordering[kx2], 10*I, b, noiseModel::Unit::Create(2));

	// create a small linear factor graph
	GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering);

	// get the factor kf2 from the factor graph
	JacobianFactor::shared_ptr lf =
	    boost::dynamic_pointer_cast<JacobianFactor>(fg[1]);

	// check if the two factors are the same
	EXPECT(assert_equal(expected,*lf));
}

/* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleDLOptimizer )
{
  example::Graph fg(example::createReallyNonlinearFactorGraph());

  Point2 x0(3,3);
  Values c0;
  c0.insert(X(1), x0);

  Values actual = DoglegOptimizer(fg, c0).optimize();
  DOUBLES_EQUAL(0,fg.error(actual),tol);
}

/* ************************************************************************* */
int main (int argc, char* argv[]) {

  // Find default file, but if an argument is given, try loading a file
  string filename = findExampleDataFile("dubrovnik-3-7-pre");
  if (argc>1) filename = string(argv[1]);

  // Load the SfM data from file
  SfM_data mydata;
  assert(readBAL(filename, mydata));
  cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();

  // Create a factor graph
  NonlinearFactorGraph graph;

  // We share *one* noiseModel between all projection factors
  noiseModel::Isotropic::shared_ptr noise =
      noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v

  // Add measurements to the factor graph
  size_t j = 0;
  BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
    BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
      size_t i = m.first;
      Point2 uv = m.second;
      graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
    }
    j += 1;
  }

  // Add a prior on pose x1. This indirectly specifies where the origin is.
  // and a prior on the position of the first landmark to fix the scale
  graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0],  noiseModel::Isotropic::Sigma(9, 0.1)));
  graph.push_back(PriorFactor<Point3>    (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));

  // Create initial estimate
  Values initial;
  size_t i = 0; j = 0;
  BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
  BOOST_FOREACH(const SfM_Track& track, mydata.tracks)    initial.insert(P(j++), track.p);

  /* Optimize the graph and print results */
  Values result;
  try {
    LevenbergMarquardtParams params;
    params.setVerbosity("ERROR");
    LevenbergMarquardtOptimizer lm(graph, initial, params);
    result = lm.optimize();
  } catch (exception& e) {
    cout << e.what();
  }
  cout << "final error: " << graph.error(result) << endl;

  return 0;
}

/* ************************************************************************* */
TEST( NonlinearOptimizer, Factorization )
{
	Values config;
	config.insert(X(1), Pose2(0.,0.,0.));
	config.insert(X(2), Pose2(1.5,0.,0.));

	NonlinearFactorGraph graph;
	graph.add(PriorFactor<Pose2>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)));
	graph.add(BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)));

	Ordering ordering;
	ordering.push_back(X(1));
	ordering.push_back(X(2));

	LevenbergMarquardtOptimizer optimizer(graph, config, ordering);
	optimizer.iterate();

	Values expected;
	expected.insert(X(1), Pose2(0.,0.,0.));
	expected.insert(X(2), Pose2(1.,0.,0.));
	CHECK(assert_equal(expected, optimizer.values(), 1e-5));
}

/* ************************************************************************* */
TEST( SymbolicBayesNet, constructor )
{
  Ordering o; o += X(2),L(1),X(1);
  // Create manually
  IndexConditional::shared_ptr
    x2(new IndexConditional(o[X(2)],o[L(1)], o[X(1)])),
    l1(new IndexConditional(o[L(1)],o[X(1)])),
    x1(new IndexConditional(o[X(1)]));
  BayesNet<IndexConditional> expected;
  expected.push_back(x2);
  expected.push_back(l1);
  expected.push_back(x1);

  // Create from a factor graph
  GaussianFactorGraph factorGraph = createGaussianFactorGraph(o);
  SymbolicFactorGraph fg(factorGraph);

  // eliminate it
  SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate();

  CHECK(assert_equal(expected, actual));
}