#!/bin/bash
# MOLA tutorial: wheel odometry + IMU gyroscopes + RTK GNSS fusion with the MOLA smoother,
# replaying the Husky 2026-11-16 campus dataset.
#
# Usage: ./run_fusion.sh [OUTPUT_DIR]
# Options (environment variables):
#   OUTAGES=1        simulate GNSS outages (GNSS dropped 15 s of every 45 s); 0 to disable
#   IMUS=2           number of IMUs whose gyroscopes are fused: 0, 1 or 2
#   RATE=1           replay speed (e.g. 4 for 4x real time)
#   GUI=false        show the MolaViz GUI
#   BAG=...          path to the dataset bag directory
set -e
HERE=$(cd "$(dirname "$0")" && pwd)
OUT=${1:-$HERE/../results/run_$(date +%H%M%S)}
BAG=${BAG:-$HERE/../husky_2026-11-16_campus_wheels_imu_gnss}
OUTAGES=${OUTAGES:-1}
IMUS=${IMUS:-2}
RATE=${RATE:-1}
NS=/a200_0812
mkdir -p "$OUT"

# --- Smoother configuration: the robot-specific tuning of this tutorial ---
# Wheel odometry noise (MRPT motion model, see the tutorial) and a Huber kernel
# for the odometry and the GNSS factors (outliers: wheel slip, multipath):
export MOLA_ODOM_MOTION_MODEL_A1=0.05
export MOLA_ODOM_MOTION_MODEL_A4=0.10
export MOLA_ODOM_ROBUST_HUBER=3.0
export MOLA_NAVSTATE_GNSS_HUBER=3.0
# No accelerometers: tie the keyframe velocities to the poses, and start slow:
export MOLA_NAVSTATE_SIGMA_INTEGRATOR_POS=0.2
export MOLA_INITIAL_TWIST_SIGMA_LIN=1.0
export MOLA_INITIAL_TWIST_SIGMA_ANG=1.0
# Gyroscope-only preintegration (vibrating ground robot: accelerometers are not used):
if [ "$IMUS" -gt 0 ]; then
  export MOLA_SMOOTHER_IMU_PREINTEGRATION=true
  export MOLA_SMOOTHER_IMU_PREINTEGRATION_GYRO_ONLY=true
  export MOLA_SMOOTHER_IMU_PREINTEGRATION_MAX_IMUS=$IMUS
else
  export MOLA_SMOOTHER_IMU_PREINTEGRATION=false
fi
# Do not use the IMU orientation (magnetometer-based yaw) as an observation:
export MOLA_IMU_ATTITUDE_SIGMA=0
# Geo-reference: estimated online, judged and tracked in ENU:
export MOLA_SE_GEOREF_CONVERGENCE_IN_ENU=true
export MOLA_SE_GEOREF_TRACK_AFTER_CONVERGENCE=true
export MOLA_SE_PUBLISH_GEOREF_CONVERGENCE=true
export MOLA_WITH_GUI=${GUI:-false}

# IMU topics (launch arguments must not be empty, so add only the ones in use):
IMU_ARGS=()
if [ "$IMUS" -ge 1 ]; then
  IMU_ARGS+=(imu_topic_name:=$NS/taobotics/imu)
fi
if [ "$IMUS" -ge 2 ]; then
  IMU_ARGS+=(imu2_topic_name:=$NS/ouster/imu)
fi
GNSS_IN=$NS/gnss/f9p/weighted
GNSS=/gnss/input

if ! command -v ros2 > /dev/null; then
  echo "Source your ROS 2 environment (with MOLA) first, e.g.: source /opt/ros/jazzy/setup.bash"
  exit 1
fi
pids=()
cleanup() {
  # SIGTERM: background jobs of a script ignore SIGINT (the bag would not be closed).
  for p in "${pids[@]}"; do
    kill -TERM "$p" 2>/dev/null || true
  done
  wait 2>/dev/null || true
}
trap cleanup EXIT

# 1) The MOLA state estimator (smoother + ROS 2 bridge). odom1_as_robot_pose:=False makes the
#    wheel odometry planar, with the wheel motion model above (True is for 3D odometry sources,
#    such as LiDAR or visual odometry):
ros2 launch mola_state_estimation_smoother ros2-state-estimator.launch.py \
  use_sim_time:=True \
  use_mola_gui:=${GUI:-false} \
  estimate_geo_reference:=True \
  odom1_topic:=$NS/platform/odom odom1_label:=odom_wheels odom1_as_robot_pose:=False \
  "${IMU_ARGS[@]}" \
  gnss_topic_name:=$GNSS > "$OUT/mola.log" 2>&1 &
pids+=($!)

# 2) GNSS input, with or without simulated outages:
OFF=15
if [ "$OUTAGES" != 1 ]; then
  OFF=0
fi
python3 "$HERE/gnss_outage_relay.py" $GNSS_IN $GNSS --period 45 --off $OFF > "$OUT/relay.log" 2>&1 &
pids+=($!)

# 3) Record the outputs:
ros2 bag record -s mcap -o "$OUT/output" --use-sim-time \
  /state_estimation/pose /state_estimation/geo_pose /state_estimator/geo_ref_metadata \
  $GNSS > "$OUT/record.log" 2>&1 &
pids+=($!)
sleep 5

# 4) Replay the dataset (tf remapped out of the robot namespace):
ros2 bag play "$BAG" --clock 100 --rate "$RATE" --disable-keyboard-controls \
  --topics $NS/platform/odom $NS/taobotics/imu $NS/ouster/imu $GNSS_IN $NS/tf_static \
  --remap $NS/tf_static:=/tf_static
sleep 5
echo "Done. Outputs in: $OUT"
