namespace Car_simulation
{
    public class WheelSystem
    {
        // Physical properties
        public float Radius { get; set; } = 0.3f; // meters
        public float Inertia { get; set; } = 2.0f; // kg·m² per wheel
        public int WheelCount { get; set; } = 4;
        public int DrivenWheels { get; set; } = 2; // 2WD

        // State
        public float WheelEnergy { get; set; } = 0f; // Joules
        public float AngularVelocity => GetOmega();
        public float RPM => GetRPM();
        public float Speed => GetSpeed();
        public float ResistanceTorque { get; set; } = 0f;

        // Calculations
        public float GetTotalInertia()
        {
            return Inertia * WheelCount;
        }

        public float GetOmega()
        {
            if (WheelEnergy <= 0 || GetTotalInertia() <= 0) return 0f;
            return MathF.Sqrt(2f * WheelEnergy / GetTotalInertia());
        }

        public float GetRPM()
        {
            return AngularVelocity * PhysicsUtil.RAD_PER_SEC_TO_RPM;
        }

        public float GetSpeed()
        {
            return AngularVelocity * Radius;
        }

        public float GetEnergyFromSpeed(float speed)
        {
            float omega = speed / Radius;
            return 0.5f * GetTotalInertia() * omega * omega;
        }

        public void SetSpeed(float speed)
        {
            WheelEnergy = GetEnergyFromSpeed(speed);
        }

        // Apply work to the wheels
        public void ApplyWork(float work)
        {
            WheelEnergy += work;
            WheelEnergy = Math.Max(WheelEnergy, 0);
        }

        public void ApplyTorque(float torque, float deltaTime)
        {
            if (torque == 0) return;
            float work = torque * AngularVelocity * deltaTime;
            ApplyWork(work);
        }

        public void ApplyResistance(float deltaTime)
        {
            if (ResistanceTorque <= 0 || AngularVelocity == 0) return;

            float omega = AngularVelocity;

            if (MathF.Abs(omega) < 0.1f)
            {
                // Check if we have enough torque to overcome static friction
                // For now, just return without applying resistance to allow startup
                return;
            }

            float resistanceSign = -MathF.Sign(omega);
            float alpha = (resistanceSign * ResistanceTorque) / GetTotalInertia();

            float omegaNew = omega + alpha * deltaTime;

            if (MathF.Sign(omegaNew) != MathF.Sign(omega))
            {
                omegaNew = 0;
            }

            float energyNew = 0.5f * GetTotalInertia() * omegaNew * omegaNew;
            WheelEnergy = Math.Max(energyNew, 0);
        }
    }
}