/// <summary>
/// Resets processing, causing it to also ignore incoming data for a moment
/// </summary>
public void Reset()
{
TrackningInProgress = false;
LastSentState = null;
NumberOfCwRotations = 0;
AveragingQueue.Clear();
TimeOffset = DateTime.Now + TimeSpan.FromSeconds(1.5);
}
/// <summary>
/// Performs averaging of currently saved states
/// </summary>
/// <param name="trackingState">Arrived state</param>
/// <returns>Averaged result</returns>
private TrackingState Averaging(TrackingState trackingState)
{
AveragingQueue.Enqueue(new TimestampObject <TrackingState>(trackingState));
var now = DateTime.Now;
// discard all states older than x seconds
var temp = AveragingQueue.ToList();
temp.RemoveAll(x => Math.Abs((now - x.Timestamp).Seconds) > 2);
AveragingQueue = new Queue <TimestampObject <TrackingState> >(temp);
// don't average if there aren't enough samples
if (AveragingQueue.Count <= 3)
{
return(null);
}
// choose most common tracking state
List <Tuple <TrackingState, int> > aggregation = new List <Tuple <TrackingState, int> >();
foreach (var state in AveragingQueue)
{
if (aggregation.Any(x => x.Item1.IsEquivalentTo(state.StoredObject)))
{
var old = aggregation.Single(x => x.Item1.IsEquivalentTo(state.StoredObject));
aggregation.Remove(old);
aggregation.Add(new Tuple <TrackingState, int>(old.Item1, old.Item2 + 1));
}
else
{
aggregation.Add(new Tuple <TrackingState, int>(state.StoredObject, 1));
}
}
return(aggregation.OrderByDescending(x => x.Item2).First().Item1);
}