#!/usr/bin/python3
# autopkgtest smoke test for python3-gtsam
# Exercises factor graph construction and optimisation through the Python API.
import sys
import math
import numpy as np
import gtsam

# ---------------------------------------------------------------------------
# 1. Basic import / version check
# ---------------------------------------------------------------------------
print(f"gtsam module loaded from: {gtsam.__file__}")

# ---------------------------------------------------------------------------
# 2. Tiny 2-D odometry problem (mirrors OdometryExample)
# ---------------------------------------------------------------------------
PRIOR_NOISE    = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))

graph = gtsam.NonlinearFactorGraph()
graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))
graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))

initial = gtsam.Values()
initial.insert(1, gtsam.Pose2(0.5,  0.0,  0.2))
initial.insert(2, gtsam.Pose2(2.3,  0.1, -0.2))
initial.insert(3, gtsam.Pose2(4.1,  0.1,  0.1))

result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize()

x3 = result.atPose2(3)
assert math.isclose(x3.x(), 4.0, abs_tol=0.1), f"x3.x() unexpected: {x3.x()}"
assert math.isclose(x3.y(), 0.0, abs_tol=0.1), f"x3.y() unexpected: {x3.y()}"
print(f"smoke-python PASSED  x3=({x3.x():.4f}, {x3.y():.4f}, {x3.theta():.4f})")

sys.exit(0)
