namespace Car_simulation
{
    public class WheelSystem
    {
        // Physical properties
        public float Radius { get; set; } = 0.3f; // meters
        public float WheelInertia { get; set; } = 2.0f; // kg·m² per wheel
        public float CarMass { get; set; } = 1500f; // kg - Car mass integrated into wheel system

        public int WheelCount { get; set; } = 4;
        public int DrivenWheels { get; set; } = 2; // 2WD

        // State
        public float TotalEnergy { get; set; } = 0f; // Joules (rotational + translational)
        public float AngularVelocity => GetOmega();
        public float RPM => GetRPM();
        public float CarSpeed => GetCarSpeed(); // Now returns actual car speed

        public float ResistanceTorque { get; set; } = 0f;

        // Calculations
        public float GetTotalRotationalInertia()
        {
            return WheelInertia * WheelCount;
        }

        public float GetEquivalentCarInertia()
        {
            // Convert car mass to equivalent rotational inertia at wheels
            // I = m * r² (from v = ω * r, so KE_translational = 0.5 * m * v² = 0.5 * m * (ωr)² = 0.5 * m * r² * ω²)
            return CarMass * Radius * Radius;
        }

        public float GetTotalInertia()
        {
            // Total inertia = rotational inertia of wheels + equivalent inertia of car mass
            return GetTotalRotationalInertia() + GetEquivalentCarInertia();
        }

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

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

        public float GetCarSpeed()
        {
            // v = ω * r (no slip assumed for base calculation)
            return AngularVelocity * Radius;
        }

        public float GetRotationalEnergy()
        {
            // Just the energy from wheel rotation
            float omega = GetOmega();
            return 0.5f * GetTotalRotationalInertia() * omega * omega;
        }

        public float GetTranslationalEnergy()
        {
            // Just the energy from car motion
            float speed = GetCarSpeed();
            return 0.5f * CarMass * speed * speed;
        }

        public float GetEnergyFromSpeed(float speed)
        {
            // Calculate total energy for given car speed
            // Total energy = rotational energy of wheels + translational energy of car
            float omega = speed / Radius;
            float rotationalEnergy = 0.5f * GetTotalRotationalInertia() * omega * omega;
            float translationalEnergy = 0.5f * CarMass * speed * speed;
            return rotationalEnergy + translationalEnergy;
        }

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

        // Apply work to the entire system (wheels + car)
        public void ApplyWork(float work)
        {
            TotalEnergy += work;
            TotalEnergy = Math.Max(TotalEnergy, 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)
            {
                // Static friction - 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;
            TotalEnergy = Math.Max(energyNew, 0);
        }
    }
}