using RobotNet10.Shared.Numbers;
using SysNum = System.Numerics;

namespace RobotNet10.RobotApp.Services.Navigation.CSharp;

/// <summary>
/// Circular buffer để lưu lịch sử (fixed size)
/// </summary>
public class CircularBuffer<T>(int capacity) where T : SysNum.INumber<T>
{
    private readonly T[] _buffer = new T[capacity];
    private int _head = 0;
    private int _count = 0;
    private readonly int _capacity = capacity;

    public int Count => _count;
    public int Capacity => _capacity;

    public void Add(T item)
    {
        _buffer[_head] = item;
        _head = (_head + 1) % _capacity;

        if (_count < _capacity)
            _count++;
    }

    public void Clear()
    {
        _head = 0;
        _count = 0;
        Array.Clear(_buffer, 0, _capacity);
    }

    public T[] ToArray()
    {
        T[] result = new T[_count];
        for (int i = 0; i < _count; i++)
        {
            int index = (_head - _count + i + _capacity) % _capacity;
            result[i] = _buffer[index];
        }
        return result;
    }

    public double Average()
    {
        if (_count == 0) return 0;

        T sum = T.Zero;
        for (int i = 0; i < _count; i++)
        {
            int index = (_head - _count + i + _capacity) % _capacity;
            sum += _buffer[index];
        }
        return double.CreateChecked(sum) / _count;
    }
}