XFEEDToolBox

namespace XFEEDToolBox.Core.Navigation;

/// <summary>Derives heat slope from one consistent update phase without discarding fast samples' baseline.</summary>
public sealed class ScoHeatSampleHistory
{
    private ulong baselineAt,lastObservedAt;
    private double baselineHeat;
    private readonly Queue<(ulong At,double Rise)> positiveSlopes=new();
    public double Slope{get;private set;}=double.NaN;
    public ulong SlopeStartAt{get;private set;}
    public ulong SlopeEndAt{get;private set;}
    public double SlopeStartHeat{get;private set;}=double.NaN;
    public double SlopeEndHeat{get;private set;}=double.NaN;
    public double RecentPeakRise{get;private set;}=double.NaN;
    public ulong RecentPeakExpiresAt{get;private set;}
    public double ConservativeSlope(ulong at)
    {
        var slope=Slope;
        foreach(var point in positiveSlopes)
            if(point.At<=at&&at-point.At<=250)slope=Math.Max(slope,point.Rise);
        return slope;
    }

    public void Reset()
    {
        baselineAt=lastObservedAt=SlopeStartAt=SlopeEndAt=RecentPeakExpiresAt=0;baselineHeat=0;
        Slope=SlopeStartHeat=SlopeEndHeat=RecentPeakRise=double.NaN;
        positiveSlopes.Clear();
    }

    /// <returns>True only when a new slope was calculated.</returns>
    public bool Observe(ulong at,double heat)
    {
        if(at==0||!double.IsFinite(heat)||heat<0)
        {Reset();return false;}
        if(baselineAt==0||at<lastObservedAt||at-lastObservedAt>1000||at-baselineAt>1000)
        {
            Reset();baselineAt=lastObservedAt=at;baselineHeat=heat;return false;
        }
        if(at==lastObservedAt)return false;
        lastObservedAt=at;
        var elapsed=at-baselineAt;
        // Keep the earlier distinct sample until the interval can support a
        // derivative. Advancing it on every 4–8 ms callback starves the slope.
        if(elapsed<10)return false;
        Slope=(heat-baselineHeat)/(elapsed/1000d);
        SlopeStartAt=baselineAt;SlopeEndAt=at;SlopeStartHeat=baselineHeat;SlopeEndHeat=heat;
        // Keep a real rolling window: a lower positive slope cannot renew an
        // older peak. At least 10 ms between slopes bounds this queue to 26.
        while(positiveSlopes.Count>0&&at-positiveSlopes.Peek().At>250)positiveSlopes.Dequeue();
        if(Slope>0)positiveSlopes.Enqueue((at,Slope));
        RecentPeakRise=double.NaN;RecentPeakExpiresAt=0;
        foreach(var point in positiveSlopes)
            if(!double.IsFinite(RecentPeakRise)||point.Rise>=RecentPeakRise)
            {RecentPeakRise=point.Rise;RecentPeakExpiresAt=point.At+250;}
        baselineAt=at;baselineHeat=heat;return true;
    }
}