FluidSim/Components/Crankshaft.cs

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;     // kg·m²
        public float FrictionConstant;   // N·m
        public float FrictionViscous;    // N·m per rad/s

        public float LastNetTorque { get; private set; }
        public float AveragePower { get; private set; }     // smoothed, watts
        public float AverageTorque { get; private set; }    // smoothed, Nm

        private float externalTorque;
        private float _loadTorque;       // external brake torque (Nm)

        // Power averaging buffer
        private readonly float[] _powerBuffer;
        private int _powerBufIdx;
        private int _powerBufCount;
        private float _powerBufSum;

        // Torque averaging buffer (same size as power buffer)
        private readonly float[] _torqueBuffer;
        private int _torqueBufIdx;
        private int _torqueBufCount;
        private float _torqueBufSum;

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

            _powerBuffer = new float[16384];
            _torqueBuffer = new float[16384];
        }

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

        public void SetLoadTorque(float torque)
        {
            _loadTorque = Math.Max(torque, 0f);
        }

        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;

            // Internal friction torque
            float friction = FrictionConstant * MathF.Sign(AngularVelocity)
                           + FrictionViscous * AngularVelocity;

            // Net torque from gas pressure minus friction (used for power/torque display)
            float netTorque = externalTorque - friction;
            LastNetTorque = netTorque;

            // Total torque after subtracting external load (brake)
            float totalNetTorque = netTorque - _loadTorque;
            float alpha = totalNetTorque / 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;

            // ---- Power averaging ----
            float instantPower = netTorque * AngularVelocity;
            if (_powerBufCount == _powerBuffer.Length)
            {
                _powerBufSum -= _powerBuffer[_powerBufIdx];
            }
            else
            {
                _powerBufCount++;
            }
            _powerBuffer[_powerBufIdx] = instantPower;
            _powerBufSum += instantPower;
            _powerBufIdx = (_powerBufIdx + 1) % _powerBuffer.Length;
            AveragePower = _powerBufSum / _powerBufCount;

            // ---- Torque averaging ----
            if (_torqueBufCount == _torqueBuffer.Length)
            {
                _torqueBufSum -= _torqueBuffer[_torqueBufIdx];
            }
            else
            {
                _torqueBufCount++;
            }
            _torqueBuffer[_torqueBufIdx] = netTorque;
            _torqueBufSum += netTorque;
            _torqueBufIdx = (_torqueBufIdx + 1) % _torqueBuffer.Length;
            AverageTorque = _torqueBufSum / _torqueBufCount;

            externalTorque = 0f;
        }
    }
}