use r2r::{
    builtin_interfaces::msg::Duration, std_msgs::msg::Int32, trajectory_msgs::msg::*, QosProfile,
};

fn main() -> Result<(), Box<dyn std::error::Error>> {
    let ctx = r2r::Context::create()?;
    let mut node = r2r::Node::create(ctx, "testnode", "")?;
    let publisher = node.create_publisher::<JointTrajectoryPoint>("/jtp", QosProfile::default())?;
    let publisher2 = node.create_publisher::<Int32>("/native_count", QosProfile::default())?;

    // run for 10 seconds
    let mut count = 0;
    let mut positions: Vec<f64> = Vec::new();
    while count < 100 {
        positions.push(count as f64);
        let to_send = JointTrajectoryPoint {
            positions: positions.clone(),
            time_from_start: Duration {
                sec: count,
                nanosec: 0,
            },
            ..Default::default()
        };
        let mut native = r2r::NativeMsg::<Int32>::new();
        native.data = count;

        publisher.publish(&to_send).unwrap();
        publisher2.publish_native(&mut native).unwrap();

        std::thread::sleep(std::time::Duration::from_millis(100));
        count += 1;
    }

    // destroy publisher before the node
    node.destroy_publisher(publisher);

    println!("All done!");

    Ok(())
}