Car-Simulation/Car simulation/Drivetrain.cs

namespace Car_simulation
{
    public class Drivetrain
    {
        // Connected components
        public Engine Engine { get; private set; }
        public WheelSystem WheelSystem { get; private set; }

        private int currentGear = 1;
        public float[] GearRatios { get; set; } =
        {
            3.8f,   // 1st - Lower for better launch
            2.5f,   // 2nd
            1.8f,   // 3rd
            1.3f,   // 4th
            1.0f,   // 5th - Direct drive
            0.8f,   // 6th - Overdrive
            0.65f   // 7th - Double overdrive (optional)
        };
        public float FinalDriveRatio { get; set; } = 5.0f;
        public float Efficiency { get; set; } = 0.95f;
        public float ClutchEngagement { get; set; } = 0f; // 0 = disengaged, 1 = fully engaged

        // Calculated
        public float GearRatio => GetCurrentGearRatio();
        public float TotalRatio => GearRatio * FinalDriveRatio;

        // Clutch properties
        public float ClutchStiffness { get; set; } = 500f; // Nm/(rad/s) - how strongly clutch pulls speeds together
        public float MaxClutchTorque { get; set; } = 4500f; // Maximum torque clutch can transmit

        // State
        public float SpeedDifference { get; private set; } // rad/s
        public float ClutchTorque { get; private set; }
        public float TransmittedPower { get; private set; }

        private float previousWheelOmega = 0f;

        public Drivetrain(Engine engine, WheelSystem wheelSystem)
        {
            Engine = engine;
            WheelSystem = wheelSystem;
            previousWheelOmega = wheelSystem.AngularVelocity;
        }

        public void GearUp()
        {
            if (currentGear < GearRatios.Length)
            currentGear++;
        }

        public void GearDown()
        {
            if (currentGear > 1)
            currentGear--;
        }

        private float GetCurrentGearRatio()
        {
            if (currentGear == 0) return 0f; // Neutral
            if (currentGear == -1) return -3.5f; // Reverse (example ratio)
            if (currentGear > 0 && currentGear <= GearRatios.Length)
                return GearRatios[currentGear - 1];
            return 0f; // Invalid gear
        }

        public float CalculateSpeedDifference()
        {
            if (TotalRatio == 0) return 0f;

            float engineOmega = Engine.AngularVelocity;
            float wheelOmega = WheelSystem.AngularVelocity;
            float expectedWheelOmega = engineOmega / TotalRatio;

            SpeedDifference = wheelOmega - expectedWheelOmega;
            return SpeedDifference;
        }

        public float CalculateClutchTorque()
        {
            if (ClutchEngagement <= 0.01f)
            {
                ClutchTorque = 0;
                return 0f;
            }

            CalculateSpeedDifference();

            float torque = -SpeedDifference * ClutchStiffness * ClutchEngagement;
            torque = Math.Clamp(torque, -MaxClutchTorque, MaxClutchTorque);

            float actualThrottle = Engine.GetActualThrottle();
            float availableEngineTorque = Engine.GetTorqueOutput();

            float maxTorqueAtClutch = maxEngineTorque * TotalRatio * Efficiency;

            torque = maxTorqueAtClutch;


            ClutchTorque = torque;
            return torque;
        }

        public void ApplyDrivetrainWork(float deltaTime)
        {
            if (ClutchEngagement <= 0.01f || TotalRatio == 0)
            {
                ClutchTorque = 0;
                TransmittedPower = 0;
                return;
            }

            CalculateSpeedDifference();
            float clutchTorque = CalculateClutchTorque();

            bool engineDrivingWheels = clutchTorque > 0;
            bool wheelsDrivingEngine = clutchTorque < 0;

            if (engineDrivingWheels)
            {
                // Engine -> Wheels (normal driving)
                ApplyEngineToWheels(clutchTorque, deltaTime);
            }
            else if (wheelsDrivingEngine)
            {
                // Wheels -> Engine (engine braking)
                ApplyWheelsToEngine(clutchTorque, deltaTime);
            }

            TransmittedPower = clutchTorque * SpeedDifference;
        }

        private void ApplyEngineToWheels(float clutchTorque, float deltaTime)
        {
            // Existing logic for engine driving wheels
            float netWheelTorque = clutchTorque * Efficiency - WheelSystem.ResistanceTorque;
            float netEngineTorque = -clutchTorque / TotalRatio;

            // Apply to both
            Engine.ApplyTorque(netEngineTorque, deltaTime);
            WheelSystem.ApplyTorque(netWheelTorque, deltaTime);
        }

        private void ApplyWheelsToEngine(float clutchTorque, float deltaTime)
        {
            // Wheels driving engine (engine braking)
            // Negative clutchTorque means wheels are trying to spin engine faster

            float wheelTorque = clutchTorque; // Negative value
            float engineTorque = -clutchTorque / TotalRatio; // Positive resistance

            // Apply resistance to wheels
            WheelSystem.ApplyTorque(wheelTorque, deltaTime);

            Engine.ApplyTorque(-engineTorque, deltaTime); // Negative = slowing
        }

        public float GetEquivalentInertiaAtEngine()
        {
            float wheelInertia = WheelSystem.GetTotalInertia();
            return Engine.MomentOfInertia + (wheelInertia * TotalRatio * TotalRatio);
        }

        public float CalculateEngineLoad(float deltaTime)
        {
            if (ClutchEngagement <= 0.01f) return 0f;

            float wheelResistanceTorque = WheelSystem.ResistanceTorque;
            float engineLoadTorque = wheelResistanceTorque / (TotalRatio * Efficiency);

            float inertiaLoad = CalculateInertiaLoad(deltaTime);

            return engineLoadTorque + inertiaLoad;
        }

        private float CalculateInertiaLoad(float deltaTime)
        {
            float wheelAlpha = (WheelSystem.AngularVelocity - previousWheelOmega) / deltaTime;
            previousWheelOmega = WheelSystem.AngularVelocity;

            float inertiaTorque = wheelAlpha * WheelSystem.GetTotalInertia();
            return inertiaTorque / (TotalRatio * TotalRatio * Efficiency);
        }

        public void Update(float deltaTime)
        {
            ApplyDrivetrainWork(deltaTime);
        }

        // Helper methods
        public float GetSpeedDifferenceRPM()
        {
            return SpeedDifference * PhysicsUtil.RAD_PER_SEC_TO_RPM;
        }

        public string GetCurrentGearName()
        {
            return currentGear switch
            {
                -1 => "R",
                0 => "N",
                _ => currentGear.ToString()
            };
        }
    }
}