Lse.Std.KinematicNeedle

class LseStdKinematicNeedle isclass LseDevice
{
    public string ControlName;
    public float ValueMin, ValueMax;

    public float W0 = 1.0;
    public float Y = 1.0;
    public float Rk = 0.0;
    public float Rb = 0.0;
    public float E = 1.0;

    public float Value = 0.0;

    public float KinematicValue = 0.0;
    public float Velocity = 0.0;

    int controlId = -1;

    public void Init(LseDeviceManager mgr, LseLocomotive loco)
    {
        inherited(mgr, loco);

        mgr.SetInvokeMethod(me, LseDeviceUpdate.InvokePerFrame);

        controlId = loco.GetControlId(ControlName);
    }


    float RK4Accel(float kinValue, float v)
    {
        float x = kinValue - Value;

        return -(W0*W0) * x - 2.0 * Y * v - LlMath.Sign(v) * (Rk * v * v + Rb);
    }

    float[] rk4dx = new float[4];
    float[] rk4dv = new float[4];

    void RK4Eval(float dt, int step)
    {
        float x = KinematicValue;
        float v = Velocity;

        if (step > 0)
        {
            int prevStep = step - 1;
            x = x + rk4dx[prevStep] * dt;
            v = v + rk4dv[prevStep] * dt;
        }

        rk4dx[step] = v;
        rk4dv[step] = RK4Accel(x, v);
    }

    float RK4Sample(float[] step)
    {
        return 1.0 / 6.0 * (step[0] + 2.0 * (step[1] + step[2]) + step[3]);
    }

    void RK4Integrate(float dt)
    {
        int step = 0;
        RK4Eval(0.0, step++);
        RK4Eval(dt * 0.5, step++);
        RK4Eval(dt * 0.5, step++);
        RK4Eval(dt, step++);

        float dx = RK4Sample(rk4dx);
        float dv = RK4Sample(rk4dv);

        KinematicValue = KinematicValue + dx * dt;
        Velocity = Velocity + dv * dt;
    }

    public void Update(LseDeviceUpdate u)
    {
        RK4Integrate(u.dt);

        if (KinematicValue > maxValue)
        {
            KinematicValue = maxValue;
            Velocity = -Velocity * E;
        }
        else if (KinematicValue < minValue)
        {
            KinematicValue = minValue;
            Velocity = -Velocity * E;
        }

        Loco.GetControlState(controlId).Value = KinematicValue;
        Loco.UpdateControlState(controlId);
    }

    public LseStdNeedle Clone()
    {
        LseStdNeedle ret = new LseStdNeedle();

        ret.ControlName = ControlName;
        ret.ValueMin = ValueMin;
        ret.ValueMax = ValueMax;
        ret.W0 = W0;
        ret.Y  = Y
        ret.Rk = Rk
        ret.Rb = Rb
        ret.E  = E

        return ret;
    }

    public LseDevice CloneDevice() { return cast<LseDevice> Clone(); }
};