Fix64.cs

using System;
using System.IO;
using UnityEngine;

namespace TrueSync {

    [Serializable]
    public partial struct FP : IEquatable<FP>, IComparable<FP>, ISerializationCallbackReceiver {

        private long m_rawValue;

        [SerializeField]
        private string _serializedValue;

        const long MAX_VALUE = long.MaxValue;
        const long MIN_VALUE = long.MinValue;
        const int NUM_BITS = 64;
        const int FRACTIONAL_PLACES = 32;
        const long ONE = 1L << FRACTIONAL_PLACES;
        const long TEN = 10L << FRACTIONAL_PLACES;
        const long HALF = 1L << (FRACTIONAL_PLACES - 1);
        const long PI_TIMES_2 = 0x6487ED511;
        const long PI = 0x3243F6A88;
        const long PI_OVER_2 = 0x1921FB544;
        const int LUT_SIZE = (int)(PI_OVER_2 >> 15);

        // Precision of this type is 2^-32, that is 2,3283064365386962890625E-10
        public static readonly decimal Precision = (decimal)(new FP(1L));//0.00000000023283064365386962890625m;
        public static readonly FP MaxValue = new FP(MAX_VALUE-1);
        public static readonly FP MinValue = new FP(MIN_VALUE+2);
        public static readonly FP One = new FP(ONE);
        public static readonly FP Ten = new FP(TEN);
        public static readonly FP Half = new FP(HALF);

        public static readonly FP Zero = new FP();
        public static readonly FP PositiveInfinity = new FP(MAX_VALUE);
        public static readonly FP NegativeInfinity = new FP(MIN_VALUE+1);
        public static readonly FP NaN = new FP(MIN_VALUE);

        public static readonly FP EN1 = FP.One / 10;
        public static readonly FP EN2 = FP.One / 100;
        public static readonly FP EN3 = FP.One / 1000;
        public static readonly FP EN4 = FP.One / 10000;
        public static readonly FP EN5 = FP.One / 100000;
        public static readonly FP EN6 = FP.One / 1000000;
        public static readonly FP EN7 = FP.One / 10000000;
        public static readonly FP EN8 = FP.One / 100000000;
        public static readonly FP Epsilon = FP.EN3;

        public static readonly FP Pi = new FP(PI);
        public static readonly FP PiOver2 = new FP(PI_OVER_2);
        public static readonly FP PiTimes2 = new FP(PI_TIMES_2);
        public static readonly FP PiInv = (FP)0.3183098861837906715377675267M;
        public static readonly FP PiOver2Inv = (FP)0.6366197723675813430755350535M;

        public static readonly FP Deg2Rad = Pi / new FP(180);

        public static readonly FP Rad2Deg = new FP(180) / Pi;

        static readonly FP LutInterval = (FP)(LUT_SIZE - 1) / PiOver2;

        public void OnBeforeSerialize() {
            _serializedValue = m_rawValue.ToString();
        }

        public void OnAfterDeserialize() {
            if (!string.IsNullOrEmpty(_serializedValue)) {
                m_rawValue = long.Parse(_serializedValue);
            }
        }

        public static int Sign(FP value) {
            return
                value.m_rawValue < 0 ? -1 :
                value.m_rawValue > 0 ? 1 :
                0;
        }


        public static FP Abs(FP value) {
            if (value.m_rawValue == MIN_VALUE) {
                return MaxValue;
            }

            // branchless implementation, see http://www.strchr.com/optimized_abs_function
            var mask = value.m_rawValue >> 63;
            return new FP((value.m_rawValue + mask) ^ mask);
        }

        public static FP FastAbs(FP value) {
            // branchless implementation, see http://www.strchr.com/optimized_abs_function
            var mask = value.m_rawValue >> 63;
            return new FP((value.m_rawValue + mask) ^ mask);
        }


        public static FP Floor(FP value) {
            // Just zero out the fractional part
            return new FP((long)((ulong)value.m_rawValue & 0xFFFFFFFF00000000));
        }

        public static FP Ceiling(FP value) {
            var hasFractionalPart = (value.m_rawValue & 0x00000000FFFFFFFF) != 0;
            return hasFractionalPart ? Floor(value) + One : value;
        }

        public static FP Round(FP value) {
            var fractionalPart = value.m_rawValue & 0x00000000FFFFFFFF;
            var integralPart = Floor(value);
            if (fractionalPart < 0x80000000) {
                return integralPart;
            }
            if (fractionalPart > 0x80000000) {
                return integralPart + One;
            }
            // if number is halfway between two values, round to the nearest even number
            // this is the method used by System.Math.Round().
            return (integralPart.m_rawValue & ONE) == 0
                       ? integralPart
                       : integralPart + One;
        }

        public static FP operator +(FP x, FP y) {
            //  if(x == PositiveInfinity || y == PositiveInfinity || x == NegativeInfinity || y == NegativeInfinity ){
            //      UnityEngine.Debug.Log("infnity +");
            //  }
            var xl = x.m_rawValue;
            var yl = y.m_rawValue;
            var sum = xl + yl;
            // if signs of operands are equal and signs of sum and x are different
            if (((~(xl ^ yl) & (xl ^ sum)) & MIN_VALUE) != 0) {
                sum = xl > 0 ? MAX_VALUE : MIN_VALUE;
            }
            return new FP(sum);
        }

        public static FP FastAdd(FP x, FP y) {
            return new FP(x.m_rawValue + y.m_rawValue);
        }

        public static FP operator -(FP x, FP y) {
            //if(x == PositiveInfinity || y == PositiveInfinity || x == NegativeInfinity || y == NegativeInfinity ){
            //  UnityEngine.Debug.Log("infnity -");
            //}
            var xl = x.m_rawValue;
            var yl = y.m_rawValue;
            var diff = xl - yl;
            // if signs of operands are different and signs of sum and x are different
            if ((((xl ^ yl) & (xl ^ diff)) & MIN_VALUE) != 0) {
                diff = xl < 0 ? MIN_VALUE : MAX_VALUE;
            }
            return new FP(diff);
        }

        public static FP FastSub(FP x, FP y) {
            return new FP(x.m_rawValue - y.m_rawValue);
        }

        static long AddOverflowHelper(long x, long y, ref bool overflow) {
            var sum = x + y;
            // x + y overflows if sign(x) ^ sign(y) != sign(sum)
            overflow |= ((x ^ y ^ sum) & MIN_VALUE) != 0;
            return sum;
        }

        public static FP operator *(FP x, FP y) {
            //if(x == PositiveInfinity || y == PositiveInfinity || x == NegativeInfinity || y == NegativeInfinity ){
            //      UnityEngine.Debug.Log("infnity *");
            //  }
            var xl = x.m_rawValue;
            var yl = y.m_rawValue;

            var xlo = (ulong)(xl & 0x00000000FFFFFFFF);
            var xhi = xl >> FRACTIONAL_PLACES;
            var ylo = (ulong)(yl & 0x00000000FFFFFFFF);
            var yhi = yl >> FRACTIONAL_PLACES;

            var lolo = xlo * ylo;
            var lohi = (long)xlo * yhi;
            var hilo = xhi * (long)ylo;
            var hihi = xhi * yhi;

            var loResult = lolo >> FRACTIONAL_PLACES;
            var midResult1 = lohi;
            var midResult2 = hilo;
            var hiResult = hihi << FRACTIONAL_PLACES;

            bool overflow = false;
            var sum = AddOverflowHelper((long)loResult, midResult1, ref overflow);
            sum = AddOverflowHelper(sum, midResult2, ref overflow);
            sum = AddOverflowHelper(sum, hiResult, ref overflow);

            bool opSignsEqual = ((xl ^ yl) & MIN_VALUE) == 0;

            // if signs of operands are equal and sign of result is negative,
            // then multiplication overflowed positively
            // the reverse is also true
            if (opSignsEqual) {
                if (sum < 0 || (overflow && xl > 0)) {
                    return MaxValue;
                }
            } else {
                if (sum > 0) {
                    return MinValue;
                }
            }

            // if the top 32 bits of hihi (unused in the result) are neither all 0s or 1s,
            // then this means the result overflowed.
            var topCarry = hihi >> FRACTIONAL_PLACES;
            if (topCarry != 0 && topCarry != -1 /*&& xl != -17 && yl != -17*/) {
                return opSignsEqual ? MaxValue : MinValue;
            }

            // If signs differ, both operands' magnitudes are greater than 1,
            // and the result is greater than the negative operand, then there was negative overflow.
            if (!opSignsEqual) {
                long posOp, negOp;
                if (xl > yl) {
                    posOp = xl;
                    negOp = yl;
                } else {
                    posOp = yl;
                    negOp = xl;
                }
                if (sum > negOp && negOp < -ONE && posOp > ONE) {
                    return MinValue;
                }
            }

            return new FP(sum);
        }

        public static FP FastMul(FP x, FP y) {

            var xl = x.m_rawValue;
            var yl = y.m_rawValue;

            var xlo = (ulong)(xl & 0x00000000FFFFFFFF);
            var xhi = xl >> FRACTIONAL_PLACES;
            var ylo = (ulong)(yl & 0x00000000FFFFFFFF);
            var yhi = yl >> FRACTIONAL_PLACES;

            var lolo = xlo * ylo;
            var lohi = (long)xlo * yhi;
            var hilo = xhi * (long)ylo;
            var hihi = xhi * yhi;

            var loResult = lolo >> FRACTIONAL_PLACES;
            var midResult1 = lohi;
            var midResult2 = hilo;
            var hiResult = hihi << FRACTIONAL_PLACES;

            var sum = (long)loResult + midResult1 + midResult2 + hiResult;
            return new FP(sum);
        }

        //[MethodImplAttribute(MethodImplOptions.AggressiveInlining)] 
        static int CountLeadingZeroes(ulong x) {
            int result = 0;
            while ((x & 0xF000000000000000) == 0) { result += 4; x <<= 4; }
            while ((x & 0x8000000000000000) == 0) { result += 1; x <<= 1; }
            return result;
        }

        public static FP operator /(FP x, FP y) {
            //  if(x == PositiveInfinity || y == PositiveInfinity || x == NegativeInfinity || y == NegativeInfinity || x == 0 || y == 0){
            //      UnityEngine.Debug.Log("infnity /");
            //  }
            var xl = x.m_rawValue;
            var yl = y.m_rawValue;

            if (yl == 0) {
                return MAX_VALUE;
                //throw new DivideByZeroException();
            }

            var remainder = (ulong)(xl >= 0 ? xl : -xl);
            var divider = (ulong)(yl >= 0 ? yl : -yl);
            var quotient = 0UL;
            var bitPos = NUM_BITS / 2 + 1;


            // If the divider is divisible by 2^n, take advantage of it.
            while ((divider & 0xF) == 0 && bitPos >= 4) {
                divider >>= 4;
                bitPos -= 4;
            }

            while (remainder != 0 && bitPos >= 0) {
                int shift = CountLeadingZeroes(remainder);
                if (shift > bitPos) {
                    shift = bitPos;
                }
                remainder <<= shift;
                bitPos -= shift;

                var div = remainder / divider;
                remainder = remainder % divider;
                quotient += div << bitPos;

                // Detect overflow
                if ((div & ~(0xFFFFFFFFFFFFFFFF >> bitPos)) != 0) {
                    return ((xl ^ yl) & MIN_VALUE) == 0 ? MaxValue : MinValue;
                }

                remainder <<= 1;
                --bitPos;
            }

            // rounding
            ++quotient;
            var result = (long)(quotient >> 1);
            if (((xl ^ yl) & MIN_VALUE) != 0) {
                result = -result;
            }

            return new FP(result);
        }

        public static FP operator %(FP x, FP y) {
            return new FP(
                x.m_rawValue == MIN_VALUE & y.m_rawValue == -1 ?
                0 :
                x.m_rawValue % y.m_rawValue);
        }

        public static FP FastMod(FP x, FP y) {
            return new FP(x.m_rawValue % y.m_rawValue);
        }

        public static FP operator -(FP x) {
            return x.m_rawValue == MIN_VALUE ? MaxValue : new FP(-x.m_rawValue);
        }

        public static bool operator ==(FP x, FP y) {
            return x.m_rawValue == y.m_rawValue;
        }

        public static bool operator !=(FP x, FP y) {
            return x.m_rawValue != y.m_rawValue;
        }

        public static bool operator >(FP x, FP y) {
            return x.m_rawValue > y.m_rawValue;
        }

        public static bool operator <(FP x, FP y) {
            return x.m_rawValue < y.m_rawValue;
        }

        public static bool operator >=(FP x, FP y) {
            return x.m_rawValue >= y.m_rawValue;
        }

        public static bool operator <=(FP x, FP y) {
            return x.m_rawValue <= y.m_rawValue;
        }


        public static FP Sqrt(FP x) {
            var xl = x.m_rawValue;
            if (xl < 0) {
                // We cannot represent infinities like Single and Double, and Sqrt is
                // mathematically undefined for x < 0. So we just throw an exception.
                throw new ArgumentOutOfRangeException("Negative value passed to Sqrt", "x");
            }

            var num = (ulong)xl;
            var result = 0UL;

            // second-to-top bit
            var bit = 1UL << (NUM_BITS - 2);

            while (bit > num) {
                bit >>= 2;
            }

            // The main part is executed twice, in order to avoid
            // using 128 bit values in computations.
            for (var i = 0; i < 2; ++i) {
                // First we get the top 48 bits of the answer.
                while (bit != 0) {
                    if (num >= result + bit) {
                        num -= result + bit;
                        result = (result >> 1) + bit;
                    } else {
                        result = result >> 1;
                    }
                    bit >>= 2;
                }

                if (i == 0) {
                    // Then process it again to get the lowest 16 bits.
                    if (num > (1UL << (NUM_BITS / 2)) - 1) {
                        // The remainder 'num' is too large to be shifted left
                        // by 32, so we have to add 1 to result manually and
                        // adjust 'num' accordingly.
                        // num = a - (result + 0.5)^2
                        //       = num + result^2 - (result + 0.5)^2
                        //       = num - result - 0.5
                        num -= result;
                        num = (num << (NUM_BITS / 2)) - 0x80000000UL;
                        result = (result << (NUM_BITS / 2)) + 0x80000000UL;
                    } else {
                        num <<= (NUM_BITS / 2);
                        result <<= (NUM_BITS / 2);
                    }

                    bit = 1UL << (NUM_BITS / 2 - 2);
                }
            }
            // Finally, if next bit would have been 1, round the result upwards.
            if (num > result) {
                ++result;
            }
            return new FP((long)result);
        }

        public static FP Sin(FP x) {
            bool flipHorizontal, flipVertical;
            var clampedL = ClampSinValue(x.m_rawValue, out flipHorizontal, out flipVertical);
            var clamped = new FP(clampedL);

            // Find the two closest values in the LUT and perform linear interpolation
            // This is what kills the performance of this function on x86 - x64 is fine though
            var rawIndex = FastMul(clamped, LutInterval);
            var roundedIndex = Round(rawIndex);
            var indexError = 0;//FastSub(rawIndex, roundedIndex);

            var nearestValue = new FP(SinLut[flipHorizontal ?
                SinLut.Length - 1 - (int)roundedIndex :
                (int)roundedIndex]);
            var secondNearestValue = new FP(SinLut[flipHorizontal ?
                SinLut.Length - 1 - (int)roundedIndex - Sign(indexError) :
                (int)roundedIndex + Sign(indexError)]);

            var delta = FastMul(indexError, FastAbs(FastSub(nearestValue, secondNearestValue))).m_rawValue;
            var interpolatedValue = nearestValue.m_rawValue + (flipHorizontal ? -delta : delta);
            var finalValue = flipVertical ? -interpolatedValue : interpolatedValue;
            FP a2 = new FP(finalValue);
            return a2;
        }

        public static FP FastSin(FP x) {
            bool flipHorizontal, flipVertical;
            var clampedL = ClampSinValue(x.m_rawValue, out flipHorizontal, out flipVertical);

            // Here we use the fact that the SinLut table has a number of entries
            // equal to (PI_OVER_2 >> 15) to use the angle to index directly into it
            var rawIndex = (uint)(clampedL >> 15);
            if (rawIndex >= LUT_SIZE) {
                rawIndex = LUT_SIZE - 1;
            }
            var nearestValue = SinLut[flipHorizontal ?
                SinLut.Length - 1 - (int)rawIndex :
                (int)rawIndex];
            return new FP(flipVertical ? -nearestValue : nearestValue);
        }



        //[MethodImplAttribute(MethodImplOptions.AggressiveInlining)] 
        static long ClampSinValue(long angle, out bool flipHorizontal, out bool flipVertical) {
            // Clamp value to 0 - 2*PI using modulo; this is very slow but there's no better way AFAIK
            var clamped2Pi = angle % PI_TIMES_2;
            if (angle < 0) {
                clamped2Pi += PI_TIMES_2;
            }

            // The LUT contains values for 0 - PiOver2; every other value must be obtained by
            // vertical or horizontal mirroring
            flipVertical = clamped2Pi >= PI;
            // obtain (angle % PI) from (angle % 2PI) - much faster than doing another modulo
            var clampedPi = clamped2Pi;
            while (clampedPi >= PI) {
                clampedPi -= PI;
            }
            flipHorizontal = clampedPi >= PI_OVER_2;
            // obtain (angle % PI_OVER_2) from (angle % PI) - much faster than doing another modulo
            var clampedPiOver2 = clampedPi;
            if (clampedPiOver2 >= PI_OVER_2) {
                clampedPiOver2 -= PI_OVER_2;
            }
            return clampedPiOver2;
        }

        public static FP Cos(FP x) {
            var xl = x.m_rawValue;
            var rawAngle = xl + (xl > 0 ? -PI - PI_OVER_2 : PI_OVER_2);
            FP a2 = Sin(new FP(rawAngle));
            return a2;
        }

        public static FP FastCos(FP x) {
            var xl = x.m_rawValue;
            var rawAngle = xl + (xl > 0 ? -PI - PI_OVER_2 : PI_OVER_2);
            return FastSin(new FP(rawAngle));
        }

        public static FP Tan(FP x) {
            var clampedPi = x.m_rawValue % PI;
            var flip = false;
            if (clampedPi < 0) {
                clampedPi = -clampedPi;
                flip = true;
            }
            if (clampedPi > PI_OVER_2) {
                flip = !flip;
                clampedPi = PI_OVER_2 - (clampedPi - PI_OVER_2);
            }

            var clamped = new FP(clampedPi);

            // Find the two closest values in the LUT and perform linear interpolation
            var rawIndex = FastMul(clamped, LutInterval);
            var roundedIndex = Round(rawIndex);
            var indexError = FastSub(rawIndex, roundedIndex);

            var nearestValue = new FP(TanLut[(int)roundedIndex]);
            var secondNearestValue = new FP(TanLut[(int)roundedIndex + Sign(indexError)]);

            var delta = FastMul(indexError, FastAbs(FastSub(nearestValue, secondNearestValue))).m_rawValue;
            var interpolatedValue = nearestValue.m_rawValue + delta;
            var finalValue = flip ? -interpolatedValue : interpolatedValue;
            FP a2 = new FP(finalValue);
            return a2;
        }

        public static FP Atan(FP y) {
            return Atan2(y, 1);
        }

        public static FP Atan2(FP y, FP x) {
            var yl = y.m_rawValue;
            var xl = x.m_rawValue;
            if (xl == 0) {
                if (yl > 0) {
                    return PiOver2;
                }
                if (yl == 0) {
                    return Zero;
                }
                return -PiOver2;
            }
            FP atan;
            var z = y / x;

            FP sm = FP.EN2 * 28;
            // Deal with overflow
            if (One + sm * z * z == MaxValue) {
                return y < Zero ? -PiOver2 : PiOver2;
            }

            if (Abs(z) < One) {
                atan = z / (One + sm * z * z);
                if (xl < 0) {
                    if (yl < 0) {
                        return atan - Pi;
                    }
                    return atan + Pi;
                }
            } else {
                atan = PiOver2 - z / (z * z + sm);
                if (yl < 0) {
                    return atan - Pi;
                }
            }
            return atan;
        }

        public static FP Asin(FP value) {
            return 2 * Atan(value / (1 + FP.Sqrt(FP.One - value * value)));
        }

        public static FP Acos(FP value) {
            return 2 * Atan(FP.Sqrt(FP.One - value * value) / (FP.One + value));
        }

        public static implicit operator FP(long value) {
            return new FP(value * ONE);
        }

        public static explicit operator long(FP value) {
            return value.m_rawValue >> FRACTIONAL_PLACES;
        }

        public static implicit operator FP(float value) {
            return new FP((long)(value * ONE));
        }

        public static explicit operator float(FP value) {
            return (float)value.m_rawValue / ONE;
        }

        public static implicit operator FP(double value) {
            return new FP((long)(value * ONE));
        }

        public static explicit operator double(FP value) {
            return (double)value.m_rawValue / ONE;
        }

        public static explicit operator FP(decimal value) {
            return new FP((long)(value * ONE));
        }

        public static implicit operator FP(int value) {
            return new FP(value * ONE);
        }

        public static explicit operator decimal(FP value) {
            return (decimal)value.m_rawValue / ONE;
        }

        public float AsFloat() {
            return (float) this;
        }

        public int AsInt() {
            return (int) this;
        }

        public long AsLong() {
            return (long)this;
        }

        public double AsDouble() {
            return (double)this;
        }

        public decimal AsDecimal() {
            return (decimal)this;
        }

        public static float ToFloat(FP value) {
            return (float)value;
        }

        public static int ToInt(FP value) {
            return (int)value;
        }

        public static FP FromFloat(float value) {
            return (FP)value;
        }

        public static bool IsInfinity(FP value) {
            return value == NegativeInfinity || value == PositiveInfinity;
        }

        public static bool IsNaN(FP value) {
            return value == NaN;
        }

        public override bool Equals(object obj) {
            return obj is FP && ((FP)obj).m_rawValue == m_rawValue;
        }

        public override int GetHashCode() {
            return m_rawValue.GetHashCode();
        }

        public bool Equals(FP other) {
            return m_rawValue == other.m_rawValue;
        }

        public int CompareTo(FP other) {
            return m_rawValue.CompareTo(other.m_rawValue);
        }

        public override string ToString() {
            return ((float)this).ToString();
        }

        public static FP FromRaw(long rawValue) {
            return new FP(rawValue);
        }

        internal static void GenerateSinLut() {
            using (var writer = new StreamWriter("Fix64SinLut.cs")) {
                writer.Write(
@"namespace FixMath.NET {
    partial struct Fix64 {
        public static readonly long[] SinLut = new[] {");
                int lineCounter = 0;
                for (int i = 0; i < LUT_SIZE; ++i) {
                    var angle = i * Math.PI * 0.5 / (LUT_SIZE - 1);
                    if (lineCounter++ % 8 == 0) {
                        writer.WriteLine();
                        writer.Write("            ");
                    }
                    var sin = Math.Sin(angle);
                    var rawValue = ((FP)sin).m_rawValue;
                    writer.Write(string.Format("0x{0:X}L, ", rawValue));
                }
                writer.Write(
@"
        };
    }
}");
            }
        }

        internal static void GenerateTanLut() {
            using (var writer = new StreamWriter("Fix64TanLut.cs")) {
                writer.Write(
@"namespace FixMath.NET {
    partial struct Fix64 {
        public static readonly long[] TanLut = new[] {");
                int lineCounter = 0;
                for (int i = 0; i < LUT_SIZE; ++i) {
                    var angle = i * Math.PI * 0.5 / (LUT_SIZE - 1);
                    if (lineCounter++ % 8 == 0) {
                        writer.WriteLine();
                        writer.Write("            ");
                    }
                    var tan = Math.Tan(angle);
                    if (tan > (double)MaxValue || tan < 0.0) {
                        tan = (double)MaxValue;
                    }
                    var rawValue = (((decimal)tan > (decimal)MaxValue || tan < 0.0) ? MaxValue : (FP)tan).m_rawValue;
                    writer.Write(string.Format("0x{0:X}L, ", rawValue));
                }
                writer.Write(
@"
        };
    }
}");
            }
        }

        public long RawValue { get { return m_rawValue; } }

        FP(long rawValue) {
            m_rawValue = rawValue;
            _serializedValue = "";
        }

        public FP(int value) {
            m_rawValue = value * ONE;
            _serializedValue = "";
        }
    }
}