using System;

namespace FluidSim.Components
{
    public class Crankshaft
    {
        public float AngularVelocity;   // rad/s
        public float CrankAngle;        // rad, 0 … 4π
        public float PreviousAngle;

        public float Inertia = 0.2f;
        public float FrictionConstant;   // N·m
        public float FrictionViscous;    // N·m per rad/s

        private float externalTorque;

        public Crankshaft(float initialRPM = 400f)
        {
            AngularVelocity = initialRPM * 2f * MathF.PI / 60f;
            CrankAngle = 0f;
            PreviousAngle = 0f;
        }

        public void AddTorque(float torque) => externalTorque += torque;

        public void Step(float dt)
        {
            if (float.IsNaN(AngularVelocity) || float.IsInfinity(AngularVelocity))
                AngularVelocity = 0f;
            if (float.IsNaN(externalTorque) || float.IsInfinity(externalTorque))
                externalTorque = 0f;

            PreviousAngle = CrankAngle;

            float friction = FrictionConstant * MathF.Sign(AngularVelocity)
                           + FrictionViscous * AngularVelocity;
            float netTorque = externalTorque - friction;
            float alpha = netTorque / Inertia;
            AngularVelocity += alpha * dt;

            if (AngularVelocity < 0f) AngularVelocity = 0f;

            CrankAngle += AngularVelocity * dt;
            if (CrankAngle >= 4f * MathF.PI)
                CrankAngle -= 4f * MathF.PI;
            else if (CrankAngle < 0f)
                CrankAngle += 4f * MathF.PI;

            externalTorque = 0f;
        }
    }
}