Whats wrong with my car auto park AI scripts?,What am I missing from my car auto park code?

Hi all,

I'm trying to create a simple AI car script that follows waypoints until it needs to park. All that stuff is sorted, the trouble I'm having is getting the car into the space smoothly. I'm using wheel colliders to steer the vehicle. Here's what I have:

Components

CarComponent - has all the movement logic + additional waypoint stuff which isn't relevant here.

ParkingSpaceComponent - Takes over control over the vehicle was triggered. Checks if the car is within it's bounds and the direction/angle of the car.

Code Inside ParkingSpaceComponent I have this function that checks the bounds of the car within the space. This works ok. Note, it's adapted from - https://forum.unity.com/threads/how-to-park-my-car-completely.882703/ - thanks @neginfinity !

 private ParkingData CheckInPlace()
     {
         var carTransform = OccupyingCar.transform;
 
         var carVectorUp = transform.InverseTransformDirection(carTransform.up);
         var carVectorForward = transform.InverseTransformDirection(carTransform.forward);
         var carVectorRight = transform.InverseTransformDirection(carTransform.right);
         var carVectorPos = transform.InverseTransformPoint(carTransform.position);      
 
         float difference = Mathf.Acos(Vector3.Dot(carTransform.forward, transform.forward));
 
         var halfWidthVector = carVectorRight * (carWidth * 0.5f);
         var halfLengthVector = carVectorForward * (carLength * 0.5f);
 
         //Corners of the car.
         var a = carVectorPos + halfWidthVector + halfLengthVector;
         var b = carVectorPos - halfWidthVector + halfLengthVector;
         var c = carVectorPos + halfWidthVector - halfLengthVector;
         var d = carVectorPos - halfWidthVector - halfLengthVector;
 
         var aabbMin = Vector3.Min(Vector3.Min(a, b), Vector3.Min(c, d));
         var aabbMax = Vector3.Max(Vector3.Max(a, b), Vector3.Max(c, d));
 
         var withinMinWidth = (aabbMin.x > -parkingLotWidth * 0.5f);
         var withinMaxWidth = (aabbMax.x < parkingLotWidth * 0.5f);
         var withinMinLength = (aabbMin.z > -parkingLotLength * 0.5f);
         var withinMaxLength = (aabbMax.z < parkingLotLength * 0.5f);
 
         //Actual overlap test:
         bool withinSpaceBounds =
             withinMinWidth
             && withinMaxWidth
             && withinMinLength
             && withinMaxLength;
 
         bool mostlyInPlace = (withinMinWidth && withinMaxWidth) && (withinMaxLength || withinMinLength);
 
         return new ParkingData(withinSpaceBounds, withinMinWidth, withinMinLength, withinMaxWidth, withinMaxLength);
     }

Then to get the steering angle I have this (which I think is where a possible issue might be?) where Direction is just Forward/Reverse.

    private float GetDifference(Direction direction)
     {
         return (direction == Direction.Forward) ? Mathf.Acos(Vector3.Dot(OccupyingCar.transform.forward, transform.forward)) : Mathf.Acos(Vector3.Dot(-OccupyingCar.transform.forward, -transform.forward)) * Mathf.Rad2Deg;
     }

Direction is determined like this

  private Direction GetCorrectingDir(ParkingData parkingData)
     {
         var f = Vector3.Dot(OccupyingCar.transform.forward, transform.forward);
         var r = Vector3.Dot(OccupyingCar.transform.right, transform.right);
 
         var alignedBackward = (f < 0 && r < 0);
 
         // if f && r < 0 then it's aligned but facing backwards
         // if f || r < 0 then it's the total wrong direction
         //     in this case, use the difference to found how to correct.
         //
         // if is facing backward, adjust difference calulation to use -Vector.forward (0,0,-1) 
         if (alignedBackward)
         {
             if (parkingData.WithinMaxLength)
                 return Direction.Reverse;
             else if (parkingData.WithinMinLength)
                 return Direction.Forward;
         }
         else
         {
             if (parkingData.WithinMaxLength)
                 return Direction.Forward;
             else if (parkingData.WithinMinLength)
                 return Direction.Reverse;
         }
 
         return Direction.Forward;
     }

Finally, to get the actual steering angle for the wheels, I have this.

   private float GetCorrectingSteer(ParkingData parkingData, Direction correctingDir)
     {       
         float steering = 0;
 
         var diff = Mathf.Clamp(GetDifference(correctingDir), -OccupyingCar.carSetting.maxSteerAngle, OccupyingCar.carSetting.maxSteerAngle);
 
         if (correctingDir == Direction.Forward)
         {  
             if (!parkingData.WithinMaxWidth)
                 steering = -OccupyingCar.carSetting.maxSteerAngle;
             else if (!parkingData.WithinMinWidth)
                 steering = OccupyingCar.carSetting.maxSteerAngle;
             else
                 steering = diff;
         }
         else
         {
             if (!parkingData.WithinMaxWidth)
                 steering = OccupyingCar.carSetting.maxSteerAngle;
             else if (!parkingData.WithinMinWidth)
                 steering = -OccupyingCar.carSetting.maxSteerAngle;
             else
                 steering = diff;
         }
         
         return steering;
     }

The idea here is that depending on which side of the lot the car is breaching, we set the turn angle to make the car move into the box.

Realise this might not be very clear but at this point I don't know what is relevant/wrong so willing to post more info if required.

Thanks in advance