use std::f64::consts::PI;
use std::time::Duration;

use anyhow::Result;
use nalgebra::{Isometry3, Quaternion, Rotation3, Translation3, UnitQuaternion, Vector3};

use rubullet::*;

fn main() -> Result<()> {
    let mut physics_client = PhysicsClient::connect(Mode::Gui)?;
    physics_client.set_additional_search_path(
        "../rubullet-sys/bullet3/libbullet3/examples/pybullet/gym/pybullet_data",
    )?;
    physics_client.configure_debug_visualizer(DebugVisualizerFlag::CovEnableYAxisUp, true);
    physics_client.set_time_step(Duration::from_secs_f64(1. / 60.));
    physics_client.set_gravity(Vector3::new(0.0, -9.8, 0.));

    let time_step = Duration::from_secs_f64(1. / 60.);
    let mut panda = PandaSim::new(&mut physics_client, Vector3::zeros())?;
    loop {
        panda.step(&mut physics_client);
        physics_client.step_simulation()?;
        std::thread::sleep(time_step);
    }
}

pub struct PandaSim {
    pub offset: Vector3<f64>,
    pub id: BodyId,
    pub t: Duration,
}

impl PandaSim {
    const INITIAL_JOINT_POSITIONS: [f64; 9] =
        [0.98, 0.458, 0.31, -2.24, -0.30, 2.66, 2.32, 0.02, 0.02];
    const PANDA_NUM_DOFS: usize = 7;
    const PANDA_END_EFFECTOR_INDEX: usize = 11;
    const LL: [f64; 9] = [-7.; 9];
    const UL: [f64; 9] = [7.; 9];
    const JR: [f64; 9] = [7.; 9];
    const NULL_SPACE_PARAMETERS: InverseKinematicsNullSpaceParameters<'static> =
        InverseKinematicsNullSpaceParameters {
            lower_limits: &PandaSim::LL,
            upper_limits: &PandaSim::UL,
            joint_ranges: &PandaSim::JR,
            rest_poses: &PandaSim::INITIAL_JOINT_POSITIONS,
        };
    pub fn new(client: &mut PhysicsClient, offset: Vector3<f64>) -> Result<Self, Error> {
        let transform = Isometry3::new(
            Vector3::new(0., 0., -0.6) + offset.clone(),
            Rotation3::from(UnitQuaternion::from_quaternion(Quaternion::new(
                0.5, -0.5, -0.5, -0.5,
            )))
            .scaled_axis(),
        );
        let urdf_options = UrdfOptions {
            base_transform: transform.clone(),
            flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES,
            ..Default::default()
        };
        client.load_urdf("tray/traybox.urdf", urdf_options)?;

        let transform = Isometry3::translation(offset.x + 0.1, offset.y + 0.3, offset.z - 0.5);
        let urdf_options = UrdfOptions {
            base_transform: transform.clone(),
            flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES,
            ..Default::default()
        };
        client.load_urdf("lego/lego.urdf", urdf_options)?;

        let transform = Isometry3::translation(offset.x - 0.1, offset.y + 0.3, offset.z - 0.5);
        let urdf_options = UrdfOptions {
            base_transform: transform.clone(),
            flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES,
            ..Default::default()
        };
        client.load_urdf("lego/lego.urdf", urdf_options)?;

        let transform = Isometry3::translation(offset.x + 0.1, offset.y + 0.3, offset.z - 0.7);
        let urdf_options = UrdfOptions {
            base_transform: transform.clone(),
            flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES,
            ..Default::default()
        };
        client.load_urdf("lego/lego.urdf", urdf_options)?;

        let transform = Isometry3::translation(offset.x, offset.y + 0.3, offset.z - 0.6);
        let urdf_options = UrdfOptions {
            base_transform: transform.clone(),
            flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES,
            ..Default::default()
        };
        client.load_urdf("sphere_small.urdf", urdf_options)?;

        let transform = Isometry3::translation(offset.x, offset.y + 0.3, offset.z - 0.5);
        let urdf_options = UrdfOptions {
            base_transform: transform.clone(),
            flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES,
            ..Default::default()
        };
        client.load_urdf("sphere_small.urdf", urdf_options)?;

        let transform = Isometry3::translation(offset.x, offset.y + 0.3, offset.z - 0.7);
        let urdf_options = UrdfOptions {
            base_transform: transform.clone(),
            flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES,
            ..Default::default()
        };
        client.load_urdf("sphere_small.urdf", urdf_options)?;
        let cube_start_position = Isometry3::new(
            offset.clone(),
            UnitQuaternion::from_euler_angles(-PI / 2., 0., 0.).scaled_axis(),
        );
        let urdf_options = UrdfOptions {
            use_fixed_base: true,
            base_transform: cube_start_position.clone(),
            flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES,
            ..Default::default()
        };
        let panda_id = client.load_urdf("franka_panda/panda.urdf", urdf_options)?;
        client.change_dynamics(
            panda_id,
            None,
            ChangeDynamicsOptions {
                linear_damping: Some(0.),
                angular_damping: Some(0.),
                ..Default::default()
            },
        );
        let mut index = 0;
        for i in 0..client.get_num_joints(panda_id) {
            let info = client.get_joint_info(panda_id, i);
            if info.joint_type == JointType::Revolute || info.joint_type == JointType::Prismatic {
                client.reset_joint_state(
                    panda_id,
                    i,
                    PandaSim::INITIAL_JOINT_POSITIONS[index],
                    None,
                )?;
                index += 1;
            }
        }
        let t = Duration::new(0, 0);
        Ok(PandaSim {
            offset,
            id: panda_id,
            t,
        })
    }
    pub fn step(&mut self, client: &mut PhysicsClient) {
        let t = self.t.as_secs_f64();
        self.t += Duration::from_secs_f64(1. / 60.);

        let pose = Isometry3::from_parts(
            Translation3::new(
                0.2 * f64::sin(1.5 * t),
                0.044,
                -0.6 + 0.1 * f64::cos(1.5 * t),
            ),
            UnitQuaternion::<f64>::from_euler_angles(PI / 2., 0., 0.),
        );
        let inverse_kinematics_parameters =
            InverseKinematicsParametersBuilder::new(PandaSim::PANDA_END_EFFECTOR_INDEX, &pose)
                .set_max_num_iterations(5)
                .use_null_space(PandaSim::NULL_SPACE_PARAMETERS)
                .build();
        let joint_poses = client
            .calculate_inverse_kinematics(self.id, inverse_kinematics_parameters)
            .unwrap();
        for i in 0..PandaSim::PANDA_NUM_DOFS {
            client.set_joint_motor_control(
                self.id,
                i,
                ControlCommand::Position(joint_poses[i]),
                Some(240. * 5.),
            );
        }
    }
}