namespace SJK.Math;

using System;
using System.Collections.Generic;
using Arch.LowLevel;

public sealed class Ordered1DList<T> : IDisposable where T : unmanaged
{
  private readonly UnsafeList<Entry> _values;
  public Ordered1DList(int capacity)
  {
    _values = new(capacity);
  }
  public record struct Entry(float Position, T Value);
  public void Insert(T item, float position)
  {
    if (_values.Count == 0 || position >= _values[^1].Position)
    {
      _values.Add(new(position, item));
      return;
    }
    if (position <= _values[0].Position)
    {
      _values.Insert(0, new(position, item));
      return;
    }
    var index = LowerBound(position);
    _values.Insert(index, new(position, item));
    SortFromIndex(index);
  }
  public void RemoveAt(int index) => _values.RemoveAt(index);
  public void Clear() => _values.Clear();
  public void MoveAllBy(float offset)
  {
    for (int i = 0; i < _values.Count; i++)
    {
      _values[i].Position = _values[i].Position + offset;
    }
  }
  private void SortFromIndex(int index)
  {
    var entry = _values[index];

    while (index > 0 &&
    _values[index - 1].Position > entry.Position)
    {
      Swap(index, index - 1);
      index--;
    }

    // bubble right
    while (index < _values.Count - 1 &&
    _values[index + 1].Position < entry.Position)
    {
      Swap(index, index + 1);
      index++;
    }
  }
  private void Swap(int a, int b) => (_values[b], _values[a]) = (_values[a], _values[b]);
  private int LowerBound(float pos)
  {
    int lo = 0;
    int hi = _values.Count;

    while (lo < hi)
    {
      int mid = (lo + hi) >> 1;

      if (_values[mid].Position < pos)
      {
        lo = mid + 1;
      }
      else
      {
        hi = mid;
      }
    }

    return lo;
  }
  public Enumerator EnumerateTowardEnd(int startIndex) => new(this, towardsEnd: true, startIndex);
  public Enumerator EnumerateTowardStart(int startIndex) => new(this, towardsEnd: false, startIndex);
  public ref struct Enumerator
  {
    public Enumerator(Ordered1DList<T> list, bool towardsEnd, int index)
    {
      _list = list;
      _towardsEnd = towardsEnd;
      _index = index;
    }
    private readonly Ordered1DList<T> _list;
    private readonly bool _towardsEnd;
    private int _index;

    public bool MoveNext()
    {
      _index += _towardsEnd ? 1 : -1;
      return _index >= 0 && _index < _list._values.Count;
    }
    public void Remove()
    {
      _list._values.RemoveAt(_index);
      _index -= _towardsEnd ? 1 : -1;
    }

    public readonly ref Entry Current => ref _list._values[_index];
  }
  public void Dispose() => _values.Dispose();
}