#!/usr/bin/env python3

import argparse

from leo_fw.nodes import ImuCalibrator

import rclpy

rclpy.init()

parser = argparse.ArgumentParser(
    description="Calculate imu bias and send it to firmware_message_converter.",
    formatter_class=argparse.ArgumentDefaultsHelpFormatter,
)

parser.add_argument(
    "duration",
    default=10.0,
    type=float,
    nargs="?",
    help="The duration in seconds for which the IMU data is collected.",
)

args = parser.parse_args()

calib = ImuCalibrator(args.duration)

while calib.running and rclpy.ok():
    rclpy.spin_once(calib)

calib.send_bias()

calib.get_logger().info("Finishing node.")
calib.destroy_node()
rclpy.shutdown()
