#!/bin/sh
set -e

cd "$AUTOPKGTEST_TMP"

cat > test.cpp <<'END'
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/slam2d/edge_se2.h>
#include <g2o/types/slam2d/vertex_se2.h>

#include <cmath>
#include <iostream>
#include <memory>

int main() {
  using BlockSolver = g2o::BlockSolver<g2o::BlockSolverTraits<-1, -1>>;
  using LinearSolver = g2o::LinearSolverEigen<BlockSolver::PoseMatrixType>;

  g2o::SparseOptimizer optimizer;
  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(
      std::make_unique<BlockSolver>(std::make_unique<LinearSolver>())));

  auto* v0 = new g2o::VertexSE2;
  v0->setId(0);
  v0->setEstimate(g2o::SE2(0, 0, 0));
  v0->setFixed(true);
  optimizer.addVertex(v0);

  auto* v1 = new g2o::VertexSE2;
  v1->setId(1);
  v1->setEstimate(g2o::SE2(0.5, -0.3, 0.2));
  optimizer.addVertex(v1);

  auto* e = new g2o::EdgeSE2;
  e->setVertex(0, v0);
  e->setVertex(1, v1);
  e->setMeasurement(g2o::SE2(1, 0, 0));
  e->setInformation(Eigen::Matrix3d::Identity());
  optimizer.addEdge(e);

  optimizer.initializeOptimization();
  optimizer.optimize(10);

  const Eigen::Vector3d r = v1->estimate().toVector();
  std::cout << "result: " << r.transpose() << std::endl;
  if (std::abs(r[0] - 1) > 1e-6 || std::abs(r[1]) > 1e-6 ||
      std::abs(r[2]) > 1e-6)
    return 1;
  return 0;
}
END

g++ -std=c++17 -o test test.cpp $(pkgconf --cflags eigen3 spdlog) \
  -lg2o_types_slam2d -lg2o_core -lg2o_stuff $(pkgconf --libs spdlog)
./test
