From c1941205387d107fa13f8b7f59251308ba3ecf01 Mon Sep 17 00:00:00 2001
From: Joe Ardent <code@ardent.nebcorp.com>
Date: Fri, 17 Feb 2023 17:30:53 -0800
Subject: [PATCH] fix steering and braking

---
 src/action/systems.rs | 45 +++++++++++++++++++++----------------------
 src/bike/wheels.rs    |  1 +
 2 files changed, 23 insertions(+), 23 deletions(-)

diff --git a/src/action/systems.rs b/src/action/systems.rs
index b41ea03..648f824 100644
--- a/src/action/systems.rs
+++ b/src/action/systems.rs
@@ -2,13 +2,14 @@ use std::f32::consts::PI;
 #[cfg(feature = "inspector")]
 use std::time::Instant;
 
-use bevy::prelude::{
-    Commands, Entity, Quat, Query, Res, ResMut, Time, Transform, Vec3, With, Without,
+use bevy::{
+    ecs::entity,
+    prelude::{Commands, Entity, Quat, Query, Res, ResMut, Time, Transform, Vec3, With, Without},
 };
 use bevy_rapier3d::{
     prelude::{
-        CollisionGroups, ExternalForce, Group, MultibodyJoint, QueryFilter, RapierConfiguration,
-        RapierContext, ReadMassProperties, RevoluteJoint, Velocity,
+        CollisionGroups, ExternalForce, GenericJoint, Group, MultibodyJoint, QueryFilter,
+        RapierConfiguration, RapierContext, ReadMassProperties, RevoluteJoint, Velocity,
     },
     rapier::prelude::JointAxis,
 };
@@ -43,7 +44,6 @@ pub(super) fn falling_cat(
     mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CatControllerState)>,
     time: Res<Time>,
     settings: Res<CatControllerSettings>,
-    #[cfg(feature = "inspector")] mut debug_instant: ResMut<ActionDebugInstant>,
 ) {
     let (xform, mut forces, mut control_vars) = bike_query.single_mut();
     //let wup = xform.translation.normalize();
@@ -72,14 +72,6 @@ pub(super) fn falling_cat(
         forces.torque += wright * mag * 0.25;
     }
 
-    #[cfg(feature = "inspector")]
-    {
-        let now = Instant::now();
-        if (now - debug_instant.0).as_millis() > 500 {
-            //dbg!(roll_error, pitch_error, &control_vars, mag);
-            debug_instant.0 = now;
-        }
-    }
     control_vars.decay();
 }
 
@@ -93,30 +85,37 @@ pub(super) fn input_forces(
         (With<CyberBikeBody>, Without<CyberSteering>),
     >,
     mut steering_query: Query<&mut MultibodyJoint, With<CyberSteering>>,
+    #[cfg(feature = "inspector")] mut debug_instant: ResMut<ActionDebugInstant>,
 ) {
     let (xform, mut forces) = body_query.single_mut();
 
     // thrust
     let thrust = xform.forward() * input.throttle * settings.accel;
-    let point = xform.translation + xform.back();
+    let point = xform.translation + xform.back() * 0.5;
     let force = ExternalForce::at_point(thrust, point, xform.translation);
     *forces += force;
 
     // brake
     for mut motor in braking_query.iter_mut() {
         let factor = if input.brake { settings.accel } else { 0.0 };
-        motor.data = *motor.data.set_motor_velocity(JointAxis::X, 0.0, factor);
-        //dbg!(&motor);
+        motor.data = (*motor
+            .data
+            .as_revolute_mut()
+            .unwrap()
+            .set_motor_max_force(factor)
+            .set_motor_velocity(0.0, factor))
+        .into();
     }
 
     // steering
-    let angle = input.yaw * (PI / 2.0);
+    let angle = input.yaw.powf(3.0) * (PI / 4.0);
     let mut steering = steering_query.single_mut();
-    steering.data = *steering
+    steering.data = (*steering
         .data
-        .set_motor_position(JointAxis::AngY, angle, 20.0, 1.0);
-    //steering.rotation = Quat::from_axis_angle(Vec3::Y, angle +
-    // 90.0f32.to_radians()); dbg!(steering.rotation);
+        .as_revolute_mut()
+        .unwrap()
+        .set_motor_position(-angle, 100.0, 0.5))
+    .into();
 }
 
 /// Don't let the wheels get stuck underneat the planet
@@ -142,7 +141,7 @@ pub(super) fn surface_fix(
             cgroups.memberships = Group::NONE;
             cgroups.filters = Group::NONE;
             commands.entity(entity).insert(Tunneling {
-                frames: 6,
+                frames: 10,
                 dir: -hit.1.normal,
             });
         }
@@ -174,7 +173,7 @@ pub(super) fn tunnel_out(
         tunneling.frames -= 1;
         force.force = tunneling.dir * settings.gravity * 1.5 * mprops.0.mass;
         //#[cfg(feature = "inspector")]
-        dbg!(&tunneling);
+        //dbg!(&tunneling);
     }
 }
 
diff --git a/src/bike/wheels.rs b/src/bike/wheels.rs
index 5617f27..1f3ce2c 100644
--- a/src/bike/wheels.rs
+++ b/src/bike/wheels.rs
@@ -96,6 +96,7 @@ pub fn spawn_tires(
                 .insert(neck_joint)
                 .insert(steering)
                 .id();
+            dbg!(hub);
             hub
         } else {
             fork_rb_entity