#!/usr/bin/env python3

import rospy

if __name__ == "__main__":

    rospy.init_node("nicla_receiver")
    driver_version = rospy.get_param("~driver_version", "arduino")

    nicla_ros_publisher = None

    if driver_version == "arduino":
        from nicla_vision_ros import NiclaRosPublisher

        nicla_ros_publisher = NiclaRosPublisher()

    elif driver_version == "micropy":
        from nicla_vision_ros import NiclaRosPublisherMicroPy

        nicla_ros_publisher = NiclaRosPublisherMicroPy()

    else:
        rospy.loginfo("please provide valid 'driver_version' arg")
        exit(-1)

    rate = rospy.Rate(500)

    rospy.loginfo("Starting receiving loop")
    while not rospy.is_shutdown():

        try:
            nicla_ros_publisher.run()

        except Exception as e:
            rospy.logerr(e)
            nicla_ros_publisher.stop()
            break

        rate.sleep()

    nicla_ros_publisher.stop()
