#!/bin/sh
# autopkgtest smoke test for libgtsam-dev
# Compiles and runs a minimal C++ program that uses the installed headers
# and shared library, exercising factor graph construction and optimisation.
set -e

WORKDIR=$(mktemp -d)
trap 'rm -rf "$WORKDIR"' EXIT

cat > "$WORKDIR/test.cpp" << 'EOF'
#include <cmath>
#include <cstdio>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/inference/Symbol.h>

int main()
{
    using namespace gtsam;

    // Noise models
    auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
    auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));

    // Build a tiny 2-D odometry factor graph:  x1 -- x2 -- x3
    NonlinearFactorGraph graph;
    graph.addPrior<Pose2>(1, Pose2(0, 0, 0), priorNoise);
    graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0), odometryNoise));
    graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, 0), odometryNoise));

    // Initial estimates
    Values initial;
    initial.insert(1, Pose2(0.5, 0.0,  0.2));
    initial.insert(2, Pose2(2.3, 0.1, -0.2));
    initial.insert(3, Pose2(4.1, 0.1,  0.1));

    // Optimise
    Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();

    // Basic sanity check on the final x3 estimate
    Pose2 x3 = result.at<Pose2>(3);
    if (std::abs(x3.x() - 4.0) > 0.1 || std::abs(x3.y()) > 0.1)
    {
        std::fprintf(stderr, "Unexpected result: x=%.4f y=%.4f\n", x3.x(), x3.y());
        return 1;
    }

    std::printf("smoke-cpp PASSED  x3=(%.4f, %.4f, %.4f)\n", x3.x(), x3.y(), x3.theta());
    return 0;
}
EOF

# Find GTSAM's CMake-installed headers / libraries via pkg-config or the
# standard system paths used by the Debian package.
CFLAGS=$(pkg-config --cflags eigen3 2>/dev/null || true)
LIBS="-lgtsam -ltbb"

g++ -std=c++17 -O1 $CFLAGS -o "$WORKDIR/test" "$WORKDIR/test.cpp" $LIBS
"$WORKDIR/test"
