use anyhow::Result;
use nalgebra::{Isometry3, UnitQuaternion, Vector3};
use rubullet::*;
use std::f64::consts::PI;
use std::time::Duration;

fn main() -> Result<()> {
    let mut physics_client = PhysicsClient::connect(Mode::Gui)?;
    physics_client.set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data")?;
    physics_client.load_urdf("plane.urdf", None)?;
    let cube_id = physics_client.load_urdf(
        "cube_small.urdf",
        UrdfOptions {
            base_transform: Isometry3::translation(0., 0., 1.),
            ..Default::default()
        },
    )?;
    physics_client.set_gravity([0., 0., -10.]);
    physics_client.set_real_time_simulation(true);

    let cid = physics_client.create_constraint(
        cube_id,
        None,
        None,
        None,
        JointType::Fixed,
        [0.; 3],
        Isometry3::identity(),
        Isometry3::translation(0., 0., 1.),
    )?;
    println!("{:?}", cid);
    println!("{:?}", physics_client.get_constraint(0)?);
    let mut a = -PI;
    loop {
        a += 0.01;
        if a > PI {
            a -= PI;
        }
        std::thread::sleep(Duration::from_secs_f64(0.01));
        let change_constraint_options = ChangeConstraintOptions {
            joint_child_pivot: Some(Vector3::new(a, 0., 1.)),
            joint_child_frame_orientation: Some(UnitQuaternion::from_euler_angles(a, 0., 0.)),
            max_force: Some(50.),
            ..Default::default()
        };
        physics_client.change_constraint(cid, change_constraint_options);
    }
}