#if DEBUG
#define RANGE_CHECK
#endif

using System;
using System.Collections.Generic;
using System.Diagnostics.Contracts;
using System.Numerics;
using System.Runtime.CompilerServices;

namespace MagmaEngine.Math;

public static class FixPoint16Ext
{
    public static FixPoint16 Average(this IEnumerable<FixPoint16> source)
    {
        return Average(source, f => f);
    }

    public static FixPoint16 Average<TSource>(this IEnumerable<TSource> source, Func<TSource, FixPoint16> selector)
    {
        using IEnumerator<TSource> e = source.GetEnumerator();
        if (!e.MoveNext())
            return FixPoint16.Zero;

        long sum = selector(e.Current).m_Value;
        int count = 1;
        while (e.MoveNext())
        {
            checked { sum += selector(e.Current).m_Value; }
            count++;
        }

        return new() { m_Value = (int)(sum / count) };
    }

    public static FixPoint16 Sum(this IEnumerable<FixPoint16> source)
    {
        using IEnumerator<FixPoint16> e = source.GetEnumerator();
        if (!e.MoveNext())
            return FixPoint16.Zero;

        var sum = e.Current;
        while (e.MoveNext())
        {
            sum += e.Current;
        }

        return sum;
    }
}

public partial struct FixPoint16 : IEquatable<FixPoint16>, IComparable<FixPoint16>, IAdditionOperators<FixPoint16, FixPoint16, FixPoint16>, IMultiplyOperators<FixPoint16, FixPoint16, FixPoint16>
{
    private enum EParseState
    {
        TrimStartWhitespace,
        ParseMantissa,
        ParseExponent,
        TrimEndWhitespace
    }

    public const int c_Shift = 16;
    public const int c_IntegerMin = -32768;
    public const int c_IntegerMax = 32767;
    public const long c_LongMin = -0x80000000L;
    public const long c_LongMax = 0x7fffffffL;
    public const int c_Half = 32768;
    private const int c_FractionMask = 0x0000ffff;
    public const double c_Multiplier = 65536.0;
    public const double c_Divisor = 1.0 / 65536.0;
    public const float c_MultiplierFloat = 65536.0f;
    public const float c_DivisorFloat = (float)c_Divisor;

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public FixPoint16(FixPoint16 other)
    {
        m_Value = other.m_Value;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public FixPoint16(int value)
    {
#if RANGE_CHECK
        if (value < c_IntegerMin || value > c_IntegerMax)
        {
            throw new ArithmeticException($"Integer to FixPoint argument out of range: {value}");
        }
#endif
        m_Value = value << c_Shift;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public FixPoint16(int dividend, int divisor)
    {
#if RANGE_CHECK
        if (divisor == 0)
        {
            throw new ArithmeticException("Divison by zero");
        }
#endif
        long iResult;

        if (((uint)dividend & 0x80000000U) == ((uint)divisor & 0x80000000U))
        {
            iResult = (((long)dividend << c_Shift) + (divisor / 2)) / divisor;
        }
        else
        {
            iResult = (((long)dividend << c_Shift) - (divisor / 2)) / divisor;
        }
#if RANGE_CHECK
        if (iResult < c_LongMin || iResult > c_LongMax)
        {
            throw new ArithmeticException($"Division result out of range: {iResult}");
        }
#endif
        m_Value = (int)iResult;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public FixPoint16(long dividend, long divisor)
    {
#if RANGE_CHECK
        if (divisor == 0L)
            throw new ArithmeticException("Divison by zero");
#endif
        while (dividend > (long.MaxValue >> c_Shift) || dividend < (long.MinValue >> c_Shift))
        {
            dividend >>= 1;
            divisor >>= 1;
        }

        long iResult;

        if (((ulong)dividend & 0x8000000000000000UL) == ((ulong)divisor & 0x8000000000000000UL))
            iResult = ((dividend << c_Shift) + (divisor / 2)) / divisor;
        else
            iResult = ((dividend << c_Shift) - (divisor / 2)) / divisor;

#if RANGE_CHECK
        if (iResult < c_LongMin || iResult > c_LongMax)
            throw new ArithmeticException($"Division result out of range: {iResult}");
#endif
        m_Value = (int)iResult;
    }

    public FixPoint16(double value)
    {
#if RANGE_CHECK
        if (value < c_IntegerMin || value > c_IntegerMax)
        {
            throw new ArithmeticException($"Double to FixPoint argument out of range: {value}");
        }
#endif
        if (value < 0.0)
        {
            m_Value = (int)((value * c_Multiplier) - 0.5);
        }
        else
        {
            m_Value = (int)((value * c_Multiplier) + 0.5);
        }
    }

    public FixPoint16(float value)
    {
#if RANGE_CHECK
        if (value < c_IntegerMin || value > c_IntegerMax)
        {
            throw new ArithmeticException($"Single to FixPoint argument out of range: {value}");
        }
#endif
        m_Value = 0;
        Update(value);
    }

    public static FixPoint16 FromString(ReadOnlySpan<char> number)
    {
        Parse(number, out var numerator, out var denominator);
        return FromRational(numerator, denominator);
    }

    public static bool TryParse(ReadOnlySpan<char> number, out FixPoint16 result)
    {
        if (!TryParse(number, out var numerator, out var denominator))
        {
            result = Zero;
            return false;
        }

        result = FromRational(numerator, denominator);
        return true;
    }

    public static bool TryParse(ReadOnlySpan<char> number, out long numerator, out long denominator)
    {
        try
        {
            Parse(number, out numerator, out denominator);
        }
        catch (Exception)
        {
            numerator = 0;
            denominator = 1;
            return false;
        }

        return true;
    }

    public static void Parse(ReadOnlySpan<char> number, out long numerator, out long denominator)
    {
        numerator = 0L;
        var afterDecimal = false;
        var mantissaHasDigit = false;
        var mantissaNegative = false;
        var hasExponent = false;
        var exponentHasDigit = false;
        var exponentNegative = false;
        var exponentValue = 0;
        var denominatorPower = 0;
        var state = EParseState.TrimStartWhitespace;

        checked
        {
            var i = 0;
            while (i < number.Length)
            {
                var character = number[i];
                switch (state)
                {
                    case EParseState.TrimStartWhitespace:
                    {
                        if (char.IsWhiteSpace(character))
                        {
                            ++i;
                        }
                        else
                        {
                            state = EParseState.ParseMantissa;
                        }
                        break;
                    }
                    case EParseState.ParseMantissa:
                    {
                        if (char.IsWhiteSpace(character))
                        {
                            state = EParseState.TrimEndWhitespace;
                            break;
                        }

                        switch (character)
                        {
                            case '+' or '-' when !mantissaHasDigit:
                            {
                                mantissaNegative = character == '-';
                                break;
                            }
                            case >= '0' and <= '9':
                            {
                                var digit = character - '0';
                                numerator = numerator * 10 + digit;

                                if (afterDecimal)
                                    denominatorPower += 1;

                                mantissaHasDigit = true;
                                break;
                            }
                            case '.' when !afterDecimal:
                            {
                                afterDecimal = true;
                                break;
                            }
                            case 'e' or 'E' when mantissaHasDigit:
                            {
                                hasExponent = true;
                                state = EParseState.ParseExponent;
                                break;
                            }
                            default:
                            {
                                throw new FormatException($"Invalid character '{character}' in mantissa.");
                            }
                        }

                        ++i;
                        break;
                    }
                    case EParseState.ParseExponent:
                    {
                        if (char.IsWhiteSpace(character))
                        {
                            state = EParseState.TrimEndWhitespace;
                            break;
                        }

                        switch (character)
                        {
                            case '+' or '-' when !exponentHasDigit:
                            {
                                exponentNegative = character == '-';
                                break;
                            }
                            case >= '0' and <= '9':
                            {
                                var digit = character - '0';
                                exponentValue = exponentValue * 10 + digit;
                                exponentHasDigit = true;
                                break;
                            }
                            default:
                            {
                                throw new FormatException($"Invalid character '{character}' in exponent.");
                            }
                        }

                        ++i;
                        break;
                    }
                    case EParseState.TrimEndWhitespace:
                    {
                        if (char.IsWhiteSpace(character))
                        {
                            ++i;
                        }
                        else
                        {
                            throw new FormatException($"Unexpected character '{character}' after the number.");
                        }
                        break;
                    }
                }
            }

            if (!mantissaHasDigit)
                throw new FormatException("Missing mantissa digits.");

            if (hasExponent)
            {
                if (!exponentHasDigit)
                    throw new FormatException("Missing exponent digits.");

                exponentValue = exponentNegative ? -exponentValue : exponentValue;
                if (exponentValue > 0)
                {
                    while (exponentValue-- > 0)
                    {
                        if (denominatorPower > 0)
                        {
                            denominatorPower -= 1;
                        }
                        else
                        {
                            numerator *= 10;
                        }
                    }
                }
                else
                {
                    denominatorPower -= exponentValue;
                }
            }
        }

        numerator = mantissaNegative ? -numerator : numerator;
        denominator = 1;
        for (int i = 0; i < denominatorPower; ++i)
            denominator *= 10;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 FromValue(long value)
    {
#if RANGE_CHECK
        if (value < c_LongMin || value > c_LongMax)
            throw new ArithmeticException($"Value out of range: {value}");
#endif

        return new() { m_Value = (int)value };
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 FromInt(int value)
    {
        return new(value);
    }

    public static FixPoint16 FromDouble(double value)
    {
        return new(value);
    }

    public static FixPoint16 FromFloat(float value)
    {
        return new(value);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 FromRational(int dividend, int divisor)
    {
        return new(dividend, divisor);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 FromRational(long dividend, long divisor)
    {
        return new(dividend, divisor);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public readonly int ToIntFloor()
    {
        return m_Value >> c_Shift;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public readonly int ToIntCeil()
    {
        unchecked
        {
            return (int)(((long)m_Value + c_FractionMask) >> c_Shift);
        }
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public readonly int ToIntRound()
    {
        unchecked
        {
            return (int)(((long)m_Value + c_Half) >> c_Shift);
        }
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public readonly int ToInt()
    {
        unchecked
        {
            if (m_Value < 0)
            {
                return -(-m_Value >> c_Shift);
            }

            return m_Value >> c_Shift;
        }
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public readonly short ToShort()
    {
        var value = ToInt();
        return (short)value;
    }

    public static FixPoint16 AngleFromRadian(double angle)
    {
        return new(angle * (0.5 / System.Math.PI));
    }

    public static FixPoint16 AngleFromDegrees(double angle)
    {
        return new(angle * (1.0 / 360.0));
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 AngleFromDegrees(int iAngle)
    {
        return (FixPoint16)iAngle / 360;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public readonly double ToDouble()
    {
        return c_Divisor * m_Value;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public readonly float ToFloat()
    {
        return c_DivisorFloat * m_Value;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public readonly double AngleToRadians()
    {
        return ToDouble() * (System.Math.PI * 2.0);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public readonly float AngleToRadiansFloat()
    {
        return ToFloat() * (MathF.PI * 2.0f);
    }

    public readonly double AngleToDegrees()
    {
        return ToDouble() * 360.0;
    }

    public override readonly string ToString()
    {
        return $"{ToDouble()}[0x{m_Value:x}]";
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public override readonly int GetHashCode()
    {
        return m_Value.GetHashCode();
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public readonly int CompareTo(FixPoint16 other)
    {
        return m_Value.CompareTo(other.m_Value);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public override readonly bool Equals(object? obj)
    {
        if (obj == null || obj.GetType() != typeof(FixPoint16))
        {
            return false;
        }

        return ((FixPoint16)obj).m_Value == m_Value;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public readonly bool Equals(FixPoint16 other)
    {
        return m_Value == other.m_Value;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public readonly bool IsZero()
    {
        return m_Value == 0;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static bool operator ==(FixPoint16 a, FixPoint16 b)
    {
        return a.m_Value == b.m_Value;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static bool operator !=(FixPoint16 a, FixPoint16 b)
    {
        return a.m_Value != b.m_Value;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static bool operator <(FixPoint16 a, FixPoint16 b)
    {
        return a.m_Value < b.m_Value;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static bool operator >(FixPoint16 a, FixPoint16 b)
    {
        return a.m_Value > b.m_Value;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static bool operator <=(FixPoint16 a, FixPoint16 b)
    {
        return a.m_Value <= b.m_Value;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static bool operator >=(FixPoint16 a, FixPoint16 b)
    {
        return a.m_Value >= b.m_Value;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 operator <<(FixPoint16 a, int shift)
    {
        return new() { m_Value = a.m_Value << shift };
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 operator >> (FixPoint16 a, int shift)
    {
        return new() { m_Value = a.m_Value >> shift };
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 operator +(FixPoint16 a, FixPoint16 b)
    {
        unchecked
        {
#if RANGE_CHECK
            var iResult = (long)a.m_Value + b.m_Value;

            if (iResult < c_LongMin || iResult > c_LongMax)
            {
                throw new ArithmeticException($"Addition result out of range: {iResult}");
            }

            return new() { m_Value = (int)iResult };
#else
				return new FixPoint16 { m_Value = a.m_Value + b.m_Value };
#endif
        }
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 operator -(FixPoint16 a, FixPoint16 b)
    {
        unchecked
        {
#if RANGE_CHECK
            var iResult = (long)a.m_Value - b.m_Value;

            if (iResult < c_LongMin || iResult > c_LongMax)
            {
                throw new ArithmeticException($"Substraction result out of range: {iResult}");
            }

            return new() { m_Value = (int)iResult };
#else
				return new FixPoint16 { m_Value = a.m_Value - b.m_Value };
#endif
        }
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 operator -(FixPoint16 a)
    {
        unchecked
        {
#if RANGE_CHECK
            if (a.m_Value < 0 && -a.m_Value < 0)
            {
                throw new ArithmeticException($"Negation result out of range: {a.m_Value}");
            }
#endif
            return new() { m_Value = -a.m_Value };
        }
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 operator *(FixPoint16 a, FixPoint16 b)
    {
        unchecked
        {
            var iResult = (((long)a.m_Value * b.m_Value) + c_Half) >> c_Shift;
#if RANGE_CHECK
            if (iResult < c_LongMin || iResult > c_LongMax)
            {
                throw new ArithmeticException($"Multiplication result out of range: {iResult}");
            }
#endif
            return new() { m_Value = (int)iResult };
        }
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 operator *(FixPoint16 a, int value)
    {
        unchecked
        {
#if RANGE_CHECK
            var iResult = (long)a.m_Value * value;

            if (iResult < c_LongMin || iResult > c_LongMax)
            {
                throw new ArithmeticException($"Multiplication result out of range: {iResult}");
            }

            return new() { m_Value = (int)iResult };
#else
				return new FixPoint16 { m_Value = a.m_Value * value };
#endif
        }
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 operator /(FixPoint16 a, int value)
    {
        unchecked
        {
#if RANGE_CHECK
            if (value == 0)
            {
                throw new ArithmeticException("Divison by zero");
            }
#endif
            if ((a.m_Value & 0x80000000) == (value & 0x80000000))
            {
                return new() { m_Value = (int)(((long)a.m_Value + (value / 2)) / value) };
            }

            return new() { m_Value = (int)(((long)a.m_Value - (value / 2)) / value) };
        }
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 operator /(FixPoint16 a, FixPoint16 b)
    {
        unchecked
        {
#if RANGE_CHECK
            if (b.m_Value == 0)
            {
                throw new ArithmeticException("Divison by zero");
            }
#endif
            long iResult;

            if ((a.m_Value & 0x80000000) == (b.m_Value & 0x80000000))
            {
                iResult = (((long)a.m_Value << c_Shift) + (b.m_Value / 2)) / b.m_Value;
            }
            else
            {
                iResult = (((long)a.m_Value << c_Shift) - (b.m_Value / 2)) / b.m_Value;
            }
#if RANGE_CHECK
            if (iResult < c_LongMin || iResult > c_LongMax)
            {
                throw new ArithmeticException($"Division result out of range: {iResult}");
            }
#endif
            return new() { m_Value = (int)iResult };
        }
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static implicit operator FixPoint16(int value)
    {
        return new(value);
    }

    public static explicit operator FixPoint16(double value)
    {
        return new(value);
    }

    public static explicit operator FixPoint16(float value)
    {
        return new(value);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static explicit operator int(FixPoint16 value)
    {
        return value.ToInt();
    }

    public static explicit operator double(FixPoint16 value)
    {
        return value.ToDouble();
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static explicit operator float(FixPoint16 value)
    {
        return value.ToFloat();
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Floor(FixPoint16 value)
    {
        return value.ToIntFloor();
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Ceil(FixPoint16 value)
    {
        return value.ToIntCeil();
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Round(FixPoint16 value)
    {
        return value.ToIntRound();
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static int Sign(FixPoint16 value)
    {
        return IntMath.Sign(value.m_Value);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static int SignZeroAsPositive(FixPoint16 value)
    {
        return IntMath.Sign(value.m_Value | 1);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Abs(FixPoint16 value)
    {
        return new() { m_Value = IntMath.Abs(value.m_Value) };
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Min(FixPoint16 a, FixPoint16 b)
    {
        return new() { m_Value = IntMath.Min(a.m_Value, b.m_Value) };
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Max(FixPoint16 a, FixPoint16 b)
    {
        return new() { m_Value = IntMath.Max(a.m_Value, b.m_Value) };
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Clamp(FixPoint16 value, FixPoint16 min, FixPoint16 max)
    {
        return new() { m_Value = IntMath.Clamp(value.m_Value, min.m_Value, max.m_Value) };
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Saturate(FixPoint16 value)
    {
        return Clamp(value, Zero, One);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Step(FixPoint16 value, FixPoint16 amount)
    {
        return amount >= value ? One : Zero;
    }

    /// <summary>
    ///     Interpolates between two values using a cubic equation.
    /// </summary>
    /// <param name="value1">The source value.</param>
    /// <param name="value2">The source value.</param>
    /// <param name="amount">The weighting value.</param>
    /// <returns>The interpolated value.</returns>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 SmoothStep(FixPoint16 value1, FixPoint16 value2, FixPoint16 amount)
    {
        if (value2 <= value1)
            return Step(value1, amount);

        var x = Saturate((amount - value1) / (value2 - value1));
        return x * x * (new FixPoint16(3) - x * 2);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static bool Equal(FixPoint16 a, FixPoint16 b, FixPoint16 threshold)
    {
        var fpDiff = Abs(a - b);
        return fpDiff <= threshold;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Sqrt(FixPoint16 value)
    {
        var longValue = (long)value.m_Value << c_Shift;
        return new() { m_Value = (int)IntMath.Sqrt(longValue) };
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 SqrtLong(long value)
    {
        value = IntMath.Sqrt(value);
#if RANGE_CHECK
        if (value > 0x7fffffff)
        {
            throw new ArithmeticException($"Length out of range: {value}");
        }
#endif
        return new() { m_Value = (int)value };
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static long LengthSquared(FixPoint16 a, FixPoint16 b)
    {
        var aSquared = (long)a.m_Value * a.m_Value;
        var bSquared = (long)b.m_Value * b.m_Value;
        var value = aSquared + bSquared;
#if RANGE_CHECK
        if (value < 0)
        {
            throw new ArithmeticException($"Length squared out of range: {value}");
        }
#endif

        return value;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Length(FixPoint16 a, FixPoint16 b)
    {
        var aSquared = (long)a.m_Value * a.m_Value;
        var bSquared = (long)b.m_Value * b.m_Value;
        var value = aSquared + bSquared;
#if RANGE_CHECK
        if (value < 0)
        {
            throw new ArithmeticException($"Length squared out of range: {value}");
        }
#endif

        value = IntMath.Sqrt(value);
#if RANGE_CHECK
        if (value > 0x7fffffff)
        {
            throw new ArithmeticException($"Length out of range: {value}");
        }
#endif
        return new() { m_Value = (int)value };
    }

    public static FixPoint16 Length(FixPoint16 a, FixPoint16 b, FixPoint16 c)
    {
        var aSquared = (long)a.m_Value * a.m_Value;
        var bSquared = (long)b.m_Value * b.m_Value;
        var cSquared = (long)c.m_Value * c.m_Value;
        var value = aSquared + bSquared;
#if RANGE_CHECK
        if (value < 0)
        {
            throw new ArithmeticException($"Length squared out of range: {value}");
        }
#endif
        value += cSquared;
#if RANGE_CHECK
        if (value < 0)
        {
            throw new ArithmeticException($"Length squared out of range: {value}");
        }
#endif

        value = IntMath.Sqrt(value);
#if RANGE_CHECK
        if (value > 0x7fffffff)
        {
            throw new ArithmeticException($"Length out of range: {value}");
        }
#endif
        return new() { m_Value = (int)value };
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Cos(FixPoint16 value)
    {
        return new() { m_Value = s_CosTable[value.m_Value & 0xffff] };
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Sin(FixPoint16 value)
    {
        return Cos(value - HalfPi);
    }

    // No table for Tan, uses division. Add table if performance of Tan is critical.
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Tan(FixPoint16 value)
    {
        return Sin(value) / Cos(value);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Asin(FixPoint16 value)
    {
        if (value.m_Value < 0)
        {
#if RANGE_CHECK
            if (value.m_Value < MinusOne.m_Value)
            {
                throw new ArithmeticException($"Asin number out of range: {value.m_Value}");
            }
#endif
            return new() { m_Value = -s_AsinTable[-value.m_Value] };
        }
#if RANGE_CHECK
        if (value.m_Value > One.m_Value)
        {
            throw new ArithmeticException($"Asin number out of range: {value.m_Value}");
        }
#endif
        return new() { m_Value = s_AsinTable[value.m_Value] };
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Acos(FixPoint16 value)
    {
        return HalfPi - Asin(value);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Atan(FixPoint16 value)
    {
        if (value.m_Value < 0)
        {
            if (-value.m_Value < s_AtanTable.Length)
            {
                return new() { m_Value = s_AtanTable[-value.m_Value] };
            }

            return new() { m_Value = MinusHalfPi.m_Value + s_AtanTable[-(One / value).m_Value] };
        }

        if (value.m_Value < s_AtanTable.Length)
        {
            return new() { m_Value = -s_AtanTable[value.m_Value] };
        }

        return new() { m_Value = s_AtanTable[(One / value).m_Value] };
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Atan2(FixPoint16 y, FixPoint16 x)
    {
        unchecked
        {
            if (y.m_Value == 0)
            {
                if (x.m_Value >= 0)
                {
                    return Zero;
                }

                return Pi;
            }

            if (y.m_Value > 0)
            {
                if (x.m_Value == 0)
                {
                    return HalfPi;
                }

                if (x.m_Value > 0)
                {
                    // x > 0, y > 0
                    if (y.m_Value <= x.m_Value)
                    {
                        return new() { m_Value = s_AtanTable[(y / x).m_Value] };
                    }

                    return new() { m_Value = HalfPi.m_Value - s_AtanTable[(x / y).m_Value] };
                }

                // x < 0, y > 0
                if (y.m_Value <= -x.m_Value)
                {
                    return new() { m_Value = Pi.m_Value - s_AtanTable[-(y / x).m_Value] };
                }

                return new() { m_Value = HalfPi.m_Value + s_AtanTable[-(x / y).m_Value] };
            }

            if (x.m_Value == 0)
            {
                return MinusHalfPi;
            }

            if (x.m_Value > 0)
            {
                // x > 0, y < 0
                if (-y.m_Value <= x.m_Value)
                {
                    return new() { m_Value = -s_AtanTable[-(y / x).m_Value] };
                }

                return new() { m_Value = MinusHalfPi.m_Value + s_AtanTable[-(x / y).m_Value] };
            }

            // x < 0, y < 0
            if (y.m_Value >= x.m_Value)
            {
                return new() { m_Value = MinusPi.m_Value + s_AtanTable[(y / x).m_Value] };
            }

            return new() { m_Value = MinusHalfPi.m_Value - s_AtanTable[(x / y).m_Value] };
        }
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 AngleDifference(FixPoint16 a, FixPoint16 b)
    {
        var iResult = (b.m_Value - a.m_Value) & c_FractionMask;

        if (iResult > Pi.m_Value)
        {
            iResult -= TwoPi.m_Value;
        }

        return new() { m_Value = iResult };
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 AngleDifferencePositive(FixPoint16 a, FixPoint16 b)
    {
        var iResult = (b.m_Value - a.m_Value) & c_FractionMask;
        return new() { m_Value = iResult };
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 AngleNormalize(FixPoint16 angle)
    {
        return Fract(angle + Pi) - Pi;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Lerp(FixPoint16 value1, FixPoint16 value2, FixPoint16 amount)
    {
        return value1 + (amount * (value2 - value1));
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 InvLerp(FixPoint16 amount, FixPoint16 min, FixPoint16 max)
    {
        if (min == max)
            return amount > max ? One : Zero;

        return (amount - min) / (max - min);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Remap(FixPoint16 oldValue, FixPoint16 oldMin, FixPoint16 oldMax, FixPoint16 newMin, FixPoint16 newMax)
    {
        if (oldValue >= oldMax)
            return newMax;

        if (oldValue <= oldMin)
            return newMin;

        return Lerp(newMin, newMax, InvLerp(oldValue, oldMin, oldMax));
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static FixPoint16 Fract(FixPoint16 p)
    {
        return new() { m_Value = p.m_Value & c_FractionMask };
    }

    public static FixPoint16 Sum(IEnumerable<FixPoint16> range)
    {
        var result = Zero;

        foreach (var fp in range)
            result += fp;
        return result;
    }

    public static FixPoint16 Product(IEnumerable<FixPoint16> range)
    {
        var result = One;

        foreach (var fp in range)
            result *= fp;
        return result;
    }

    public static FixPoint16 Min(IEnumerable<FixPoint16> range)
    {
        var result = MaxValue;

        foreach (var fp in range)
            result = Min(fp, result);
        return result;
    }

    public static FixPoint16 Max(IEnumerable<FixPoint16> range)
    {
        var result = MinValue;

        foreach (var fp in range)
            result = Max(fp, result);
        return result;
    }

    public void Update(float value)
    {
        if (value < 0.0f)
        {
            m_Value = (int)((value * c_MultiplierFloat) - 0.5f);
        }
        else
        {
            m_Value = (int)((value * c_MultiplierFloat) + 0.5f);
        }
    }

    public int m_Value;

    public FixPoint16 Doubled
    {
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        [Pure]
        get => new FixPoint16 { m_Value = m_Value * 2 };
    }

    public FixPoint16 Quadrupled
    {
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        [Pure]
        get => new FixPoint16 { m_Value = m_Value * 4 };
    }

    public FixPoint16 Halved
    {
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        [Pure]
        get => new FixPoint16 { m_Value = m_Value / 2 };
    }

    public FixPoint16 Quartered
    {
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        [Pure]
        get => new FixPoint16 { m_Value = m_Value / 4 };
    }

    public static readonly FixPoint16 Zero = default;
    public static readonly FixPoint16 MinValue = new() { m_Value = int.MinValue };
    public static readonly FixPoint16 MaxValue = new() { m_Value = int.MaxValue };
    public static readonly FixPoint16 Epsilon = new() { m_Value = 1 };
    public static readonly FixPoint16 EpsilonOrientation = new() { m_Value = 10 };
    public static readonly FixPoint16 MinusEpsilon = new() { m_Value = -1 };
    public static readonly FixPoint16 One = new(1);
    public static readonly FixPoint16 MinusOne = new(-1);
    public static readonly FixPoint16 Half = new() { m_Value = One.m_Value / 2 };
    public static readonly FixPoint16 Quarter = new() { m_Value = One.m_Value / 4 };
    public static readonly FixPoint16 MinusHalf = -Half;
    public static readonly FixPoint16 TwoPi = One;
    public static readonly FixPoint16 Pi = Half;
    public static readonly FixPoint16 HalfPi = new() { m_Value = Pi.m_Value / 2 };
    public static readonly FixPoint16 QuaterPi = new() { m_Value = HalfPi.m_Value / 2 };
    public static readonly FixPoint16 MinusTwoPi = -TwoPi;
    public static readonly FixPoint16 MinusPi = -Pi;
    public static readonly FixPoint16 MinusHalfPi = -HalfPi;
    public static readonly FixPoint16 MinusQuaterPi = -QuaterPi;
    public static readonly FixPoint16 InvSqrt2 = One / Sqrt(2);
}