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

 * 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 LIEKF_NavstateExample.cpp
 * @brief LIEKF on NavState (SE_2(3)) with IMU (predict) and GPS (update)
 * @date April 25, 2025
 * @authors Scott Baker, Matt Kielo, Frank Dellaert
 */

#include <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/navigation/LIEKF.h>
#include <gtsam/navigation/NavState.h>

#include <iostream>

using namespace std;
using namespace gtsam;

/**
 * @brief Left-invariant dynamics for NavState.
 * @param imu  6×1 vector [a; ω]: linear acceleration and angular velocity.
 * @return     9×1 tangent: [ω; 0₃; a].
 */
Vector9 dynamics(const Vector6& imu) {
  auto a = imu.head<3>();
  auto w = imu.tail<3>();
  Vector9 xi;
  xi << w, Vector3::Zero(), a;
  return xi;
}

/**
 * @brief GPS measurement model: returns position and its Jacobian.
 * @param X    Current state.
 * @param H    Optional 3×9 Jacobian w.r.t. X.
 * @return     3×1 position vector.
 */
Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) {
  if (H) *H << Z_3x3, Z_3x3, X.R().matrix();
  return X.t();
}

int main() {
  // Initial state & covariances
  NavState X0;  // R=I, v=0, t=0
  Matrix9 P0 = Matrix9::Identity() * 0.1;
  LIEKF<NavState> ekf(X0, P0);

  // Noise & timestep
  double dt = 1.0;
  Matrix9 Q = Matrix9::Identity() * 0.01;
  Matrix3 R = Matrix3::Identity() * 0.5;

  // Two IMU samples [a; ω]
  Vector6 imu1;
  imu1 << 0.1, 0, 0, 0, 0.2, 0;
  Vector6 imu2;
  imu2 << 0, 0.3, 0, 0.4, 0, 0;

  // Two GPS fixes
  Vector3 z1;
  z1 << 0.3, 0, 0;
  Vector3 z2;
  z2 << 0.6, 0, 0;

  cout << "=== Init ===\nX: " << ekf.state() << "\nP: " << ekf.covariance()
       << "\n\n";

  // --- first predict/update ---
  ekf.predict(dynamics(imu1), dt, Q);
  cout << "--- After predict 1 ---\nX: " << ekf.state()
       << "\nP: " << ekf.covariance() << "\n\n";
  ekf.update(h_gps, z1, R);
  cout << "--- After update 1 ---\nX: " << ekf.state()
       << "\nP: " << ekf.covariance() << "\n\n";

  // --- second predict/update ---
  ekf.predict(dynamics(imu2), dt, Q);
  cout << "--- After predict 2 ---\nX: " << ekf.state()
       << "\nP: " << ekf.covariance() << "\n\n";
  ekf.update(h_gps, z2, R);
  cout << "--- After update 2 ---\nX: " << ekf.state()
       << "\nP: " << ekf.covariance() << "\n";

  return 0;
}