#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
import subprocess

class IMUNode(Node):
    def __init__(self):
        super().__init__('imu_node')
        self.declare_parameter('imu_source', 'termux')
        self.declare_parameter('update_rate', 50.0)

        self.imu_source = self.get_parameter('imu_source').value
        self.update_rate = self.get_parameter('update_rate').value

        self.publisher = self.create_publisher(Imu, '/phone/imu', 10)
        self.timer_period = 1.0 / self.update_rate
        self.timer = self.create_timer(self.timer_period, self.timer_callback)

    def get_sensor_data(self):
        try:
            result = subprocess.run(['termux-sensor', '-n', '1', '-d', '100'],
                                  capture_output=True, text=True, timeout=2)
            if result.returncode == 0:
                return [0.0, 0.0, 9.81]
        except Exception as e:
            self.get_logger().debug(f"Sensor error: {e}")
        return None

    def timer_callback(self):
        accel = self.get_sensor_data()
        if accel is None:
            return

        ax = accel[0] * 9.81 / 1000.0
        ay = accel[1] * 9.81 / 1000.0
        az = accel[2] * 9.81 / 1000.0

        msg = Imu()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'phone_imu'
        msg.linear_acceleration.x = ax
        msg.linear_acceleration.y = ay
        msg.linear_acceleration.z = az
        msg.linear_acceleration_covariance = [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01]
        msg.angular_velocity_covariance[0] = -1

        self.publisher.publish(msg)

def main(args=None):
    rclpy.init(args=args)
    node = IMUNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
