using System.Numerics;

namespace Encompass.Collections
{
    public static class BitSet512Builder
    {
        public static BitSet512 Zeroes()
        {
            return new BitSet512(Vector128Builder.Zeroes(), Vector128Builder.Zeroes(), Vector128Builder.Zeroes(), Vector128Builder.Zeroes());
        }

        public static BitSet512 Ones()
        {
            return new BitSet512(Vector128Builder.Ones(), Vector128Builder.Ones(), Vector128Builder.Ones(), Vector128Builder.Ones());
        }
    }

    public static class Vector128Builder
    {
        static readonly uint[] zeroes = new uint[4]; // max size of a Vector<T> is 8 uints
        static readonly uint[] ones = new uint[4] { uint.MaxValue, uint.MaxValue, uint.MaxValue, uint.MaxValue };
        static uint[] builderInts = new uint[4];

        private static void ResetBuilder()
        {
            for (var i = 0; i < 4; i++)
            {
                builderInts[i] = 0;
            }
        }

        public static Vector<uint> Zeroes()
        {
            return new Vector<uint>(zeroes);
        }

        public static Vector<uint> Ones()
        {
            return new Vector<uint>(ones);
        }

        public static Vector<uint> Build(int index)
        {
            if (index > 127) { throw new System.ArgumentOutOfRangeException(nameof(index)); }
            ResetBuilder();
            builderInts[index / 32] |= (uint)(1 << (index % 32));
            return new Vector<uint>(builderInts);
        }
    }

    public struct BitSet512
    {
        public Vector<uint> A { get; }
        public Vector<uint> B { get; }
        public Vector<uint> C { get; }
        public Vector<uint> D { get; }

        internal BitSet512(Vector<uint> a, Vector<uint> b, Vector<uint> c, Vector<uint> d)
        {
            A = a;
            B = b;
            C = c;
            D = d;
        }

        public BitSet512 And(BitSet512 other)
        {
            return new BitSet512(A & other.A, B & other.B, C & other.C, D & other.D);
        }

        public BitSet512 Or(BitSet512 other)
        {
            return new BitSet512(A | other.A, B | other.B, C | other.C, D | other.D);
        }

        public BitSet512 Not()
        {
            return new BitSet512(~A, ~B, ~C, ~D);
        }

        public BitSet512 Set(int index)
        {
            if (index < 128)
            {
                return new BitSet512(A | Vector128Builder.Build(index % 128), B, C, D);
            }
            else if (index < 256)
            {
                return new BitSet512(A, B | Vector128Builder.Build(index % 128), C, D);
            }
            else if (index < 384)
            {
                return new BitSet512(A, B, C | Vector128Builder.Build(index % 128), D);
            }
            else if (index < 512)
            {
                return new BitSet512(A, B, C, D | Vector128Builder.Build(index % 128));
            }
            else
            {
                throw new System.ArgumentOutOfRangeException(nameof(index));
            }
        }

        public BitSet512 UnSet(int index)
        {
            if (index < 128)
            {
                return new BitSet512(A & ~Vector128Builder.Build(index % 128), B, C, D);
            }
            else if (index < 256)
            {
                return new BitSet512(A, B & ~Vector128Builder.Build(index % 128), C, D);
            }
            else if (index < 384)
            {
                return new BitSet512(A, B, C & ~Vector128Builder.Build(index % 128), D);
            }
            else if (index < 512)
            {
                return new BitSet512(A, B, C, D & ~Vector128Builder.Build(index % 128));
            }
            else
            {
                throw new System.ArgumentOutOfRangeException(nameof(index));
            }
        }

        public bool Get(int index)
        {
            var vectorIndex = index % 128;
            if (index < 128)
            {
                return (A[vectorIndex / 32] & (uint)(1 << vectorIndex % 32)) != 0;
            }
            else if (index < 256)
            {
                return (B[vectorIndex / 32] & (uint)(1 << vectorIndex % 32)) != 0;
            }
            else if (index < 384)
            {
                return (C[vectorIndex / 32] & (uint)(1 << vectorIndex % 32)) != 0;
            }
            else if (index < 512)
            {
                return (D[vectorIndex / 32] & (uint)(1 << vectorIndex % 32)) != 0;
            }
            else
            {
                throw new System.ArgumentOutOfRangeException(nameof(index));
            }
        }

        public bool AllTrue()
        {
            for (var i = 0; i < 4; i++)
            {
                if (A[i] != uint.MaxValue) { return false; }
                if (B[i] != uint.MaxValue) { return false; }
                if (C[i] != uint.MaxValue) { return false; }
                if (D[i] != uint.MaxValue) { return false; }
            }
            return true;
        }

        public bool AllFalse()
        {
            for (var i = 0; i < 4; i++)
            {
                if (A[i] != 0) { return false; }
                if (B[i] != 0) { return false; }
                if (C[i] != 0) { return false; }
                if (D[i] != 0) { return false; }
            }
            return true;
        }
    }
}