//TODO: Expose outputs that visualizers or neural can use
//TODO: Can't just say where != null. Need to account for null times
public Tuple <TrackedItemBase, Point, Vector> GetPreviousPosition(double time)
{
// Find the two snapshots that straddle this requested time
ItemHistoryEntry low = null;
ItemHistoryEntry high = null;
foreach (ItemHistoryEntry item in _snapshots.EnumerateReverse())
{
if (item.Time.IsNearValue(time))
{
if (item.Item == null)
{
// There was no item at this time
return(null);
}
else
{
return(Tuple.Create(item.Item, item.Position_Velocity.Item1, item.Position_Velocity.Item2));
}
}
else if (item.Time < time)
{
low = item;
break;
}
else
{
high = item;
}
}
if (_snapshots.HasWrapped && (low == null || high == null))
{
_snapshots.ChangeSize(Convert.ToInt32(_snapshots.MaxCount * 1.5));
}
// Return the lerp of those two
if (low?.Item == null && high?.Item == null)
{
return(null);
}
else if (low?.Item != null && high?.Item != null)
{
double percent = (time - low.Time) / (high.Time - low.Time);
if (percent.IsInvalid()) // it should only be invalid if the denominator is zero
{
percent = .5;
}
if (low.Item.Token == high.Item.Token)
{
return(Tuple.Create(
low.Item,
Math2D.LERP(low.Position_Velocity.Item1, high.Position_Velocity.Item1, percent),
Math2D.LERP(low.Position_Velocity.Item2, high.Position_Velocity.Item2, percent)));
}
else if (percent < .5)
{
// The item tokens changed between time steps, and this time is closer to the request, so return it
return(Tuple.Create(low.Item, low.Position_Velocity.Item1, low.Position_Velocity.Item2));
}
else
{
return(Tuple.Create(high.Item, high.Position_Velocity.Item1, high.Position_Velocity.Item2));
}
}
else if (low?.Item != null)
{
return(Tuple.Create(low.Item, low.Position_Velocity.Item1, low.Position_Velocity.Item2)); // inaccurate, but as good as can be found (shouldn't happen, or very rarely)
}
else
{
// This happens when the harness is first created and being used. There hasn't been enough history yet. So return null instead of the item's current position
//return Tuple.Create(high.Item, high.Position, high.Velocity);
return(null);
}
}
