(*
    BSD 2-Clause License

    Copyright (c) 2019-2020, Anton Krotov
    All rights reserved.
*)

MODULE Math;

IMPORT SYSTEM;


CONST

    pi* = 3.1415926535897932384626433832795028841972E0;
    e*  = 2.7182818284590452353602874713526624977572E0;

    ZERO      = 0.0E0;
    ONE       = 1.0E0;
    HALF      = 0.5E0;
    TWO       = 2.0E0;
    sqrtHalf  = 0.70710678118654752440E0;
    eps       = 5.5511151E-17;
    ln2Inv    = 1.44269504088896340735992468100189213E0;
    piInv     = ONE / pi;
    Limit     = 1.0536712E-8;
    piByTwo   = pi / TWO;

    expoMax   = 1023;
    expoMin   = 1 - expoMax;


VAR

    LnInfinity, LnSmall, large, miny: REAL;


PROCEDURE [stdcall64] sqrt* (x: REAL): REAL;
BEGIN
    ASSERT(x >= ZERO);
    SYSTEM.CODE(
    0F2H, 0FH, 51H, 45H, 10H,  (*  sqrtsd  xmm0, qword[rbp + 10h]  *)
    05DH,                      (*  pop     rbp                     *)
    0C2H, 08H, 00H             (*  ret     8                       *)
    )

    RETURN 0.0
END sqrt;


PROCEDURE sqri* (x: INTEGER): INTEGER;
    RETURN x * x
END sqri;


PROCEDURE sqrr* (x: REAL): REAL;
    RETURN x * x
END sqrr;


PROCEDURE exp* (x: REAL): REAL;
CONST
    c1 =  0.693359375E0;
    c2 = -2.1219444005469058277E-4;
    P0 =  0.249999999999999993E+0;
    P1 =  0.694360001511792852E-2;
    P2 =  0.165203300268279130E-4;
    Q1 =  0.555538666969001188E-1;
    Q2 =  0.495862884905441294E-3;

VAR
    xn, g, p, q, z: REAL;
    n: INTEGER;

BEGIN
    IF x > LnInfinity THEN
        x := SYSTEM.INF()
    ELSIF x < LnSmall THEN
        x := ZERO
    ELSIF ABS(x) < eps THEN
        x := ONE
    ELSE
        IF x >= ZERO THEN
            n := FLOOR(ln2Inv * x + HALF)
        ELSE
            n := FLOOR(ln2Inv * x - HALF)
        END;

        xn := FLT(n);
        g  := (x - xn * c1) - xn * c2;
        z  := g * g;
        p  := ((P2 * z + P1) * z + P0) * g;
        q  := (Q2 * z + Q1) * z + HALF;
        x  := HALF + p / (q - p);
        PACK(x, n + 1)
    END

    RETURN x
END exp;


PROCEDURE ln* (x: REAL): REAL;
CONST
    c1 =  355.0E0 / 512.0E0;
    c2 = -2.121944400546905827679E-4;
    P0 = -0.64124943423745581147E+2;
    P1 =  0.16383943563021534222E+2;
    P2 = -0.78956112887491257267E+0;
    Q0 = -0.76949932108494879777E+3;
    Q1 =  0.31203222091924532844E+3;
    Q2 = -0.35667977739034646171E+2;

VAR
    zn, zd, r, z, w, p, q, xn: REAL;
    n: INTEGER;

BEGIN
    ASSERT(x > ZERO);

    UNPK(x, n);
    x := x * HALF;

    IF x > sqrtHalf THEN
        zn := x - ONE;
        zd := x * HALF + HALF;
        INC(n)
    ELSE
        zn := x - HALF;
        zd := zn * HALF + HALF
    END;

    z  := zn / zd;
    w  := z * z;
    q  := ((w + Q2) * w + Q1) * w + Q0;
    p  := w * ((P2 * w + P1) * w + P0);
    r  := z + z * (p / q);
    xn := FLT(n)

    RETURN (xn * c2 + r) + xn * c1
END ln;


PROCEDURE power* (base, exponent: REAL): REAL;
BEGIN
    ASSERT(base > ZERO)
    RETURN exp(exponent * ln(base))
END power;


PROCEDURE ipower* (base: REAL; exponent: INTEGER): REAL;
VAR
    i: INTEGER;
    a: REAL;

BEGIN
    a := 1.0;

    IF base # 0.0 THEN
        IF exponent # 0 THEN
            IF exponent < 0 THEN
                base := 1.0 / base
            END;
            i := ABS(exponent);
            WHILE i > 0 DO
                WHILE ~ODD(i) DO
                    i := LSR(i, 1);
                    base := sqrr(base)
                END;
                DEC(i);
                a := a * base
            END
        ELSE
            a := 1.0
        END
    ELSE
        ASSERT(exponent > 0);
        a := 0.0
    END

    RETURN a
END ipower;


PROCEDURE log* (base, x: REAL): REAL;
BEGIN
    ASSERT(base > ZERO);
    ASSERT(x > ZERO)
    RETURN ln(x) / ln(base)
END log;


PROCEDURE SinCos (x, y, sign: REAL): REAL;
CONST
    ymax =  210828714;
    c1   =  3.1416015625E0;
    c2   = -8.908910206761537356617E-6;
    r1   = -0.16666666666666665052E+0;
    r2   =  0.83333333333331650314E-2;
    r3   = -0.19841269841201840457E-3;
    r4   =  0.27557319210152756119E-5;
    r5   = -0.25052106798274584544E-7;
    r6   =  0.16058936490371589114E-9;
    r7   = -0.76429178068910467734E-12;
    r8   =  0.27204790957888846175E-14;

VAR
    n: INTEGER;
    xn, f, x1, g: REAL;

BEGIN
    ASSERT(y < FLT(ymax));

    n := FLOOR(y * piInv + HALF);
    xn := FLT(n);
    IF ODD(n) THEN
        sign := -sign
    END;
    x := ABS(x);
    IF x # y THEN
        xn := xn - HALF
    END;

    x1 := FLT(FLOOR(x));
    f  := ((x1 - xn * c1) + (x - x1)) - xn * c2;

    IF ABS(f) < Limit THEN
        x := sign * f
    ELSE
        g := f * f;
        g := (((((((r8 * g + r7) * g + r6) * g + r5) * g + r4) * g + r3) * g + r2) * g + r1) * g;
        g := f + f * g;
        x := sign * g
    END

    RETURN x
END SinCos;


PROCEDURE sin* (x: REAL): REAL;
BEGIN
    IF x < ZERO THEN
        x := SinCos(x, -x, -ONE)
    ELSE
        x := SinCos(x, x, ONE)
    END

    RETURN x
END sin;


PROCEDURE cos* (x: REAL): REAL;
    RETURN SinCos(x, ABS(x) + piByTwo, ONE)
END cos;


PROCEDURE tan* (x: REAL): REAL;
VAR
    s, c: REAL;

BEGIN
    s := sin(x);
    c := sqrt(ONE - s * s);
    x := ABS(x) / (TWO * pi);
    x := x - FLT(FLOOR(x));
    IF (0.25 < x) & (x < 0.75) THEN
        c := -c
    END

    RETURN s / c
END tan;


PROCEDURE arctan2* (y, x: REAL): REAL;
CONST
    P0 = 0.216062307897242551884E+3;  P1 = 0.3226620700132512059245E+3;
    P2 = 0.13270239816397674701E+3;   P3 = 0.1288838303415727934E+2;
    Q0 = 0.2160623078972426128957E+3; Q1 = 0.3946828393122829592162E+3;
    Q2 = 0.221050883028417680623E+3;  Q3 = 0.3850148650835119501E+2;
    Sqrt3 = 1.7320508075688772935E0;

VAR
    atan, z, z2, p, q: REAL;
    yExp, xExp, Quadrant: INTEGER;

BEGIN
    IF ABS(x) < miny THEN
        ASSERT(ABS(y) >= miny);
        atan := piByTwo
    ELSE
        z := y;
        UNPK(z, yExp);
        z := x;
        UNPK(z, xExp);

        IF yExp - xExp >= expoMax - 3 THEN
            atan := piByTwo
        ELSIF yExp - xExp < expoMin + 3 THEN
            atan := ZERO
        ELSE
            IF ABS(y) > ABS(x) THEN
                z := ABS(x / y);
                Quadrant := 2
            ELSE
                z := ABS(y / x);
                Quadrant := 0
            END;

            IF z > TWO - Sqrt3 THEN
                z := (z * Sqrt3 - ONE) / (Sqrt3 + z);
                INC(Quadrant)
            END;

            IF ABS(z) < Limit THEN
                atan := z
            ELSE
                z2 := z * z;
                p := (((P3 * z2 + P2) * z2 + P1) * z2 + P0) * z;
                q := (((z2 + Q3) * z2 + Q2) * z2 + Q1) * z2 + Q0;
                atan := p / q
            END;

            CASE Quadrant OF
            |0:
            |1: atan := atan + pi / 6.0
            |2: atan := piByTwo - atan
            |3: atan := pi / 3.0 - atan
            END
        END;

        IF x < ZERO THEN
            atan := pi - atan
        END
    END;

    IF y < ZERO THEN
        atan := -atan
    END

    RETURN atan
END arctan2;


PROCEDURE arcsin* (x: REAL): REAL;
BEGIN
    ASSERT(ABS(x) <= ONE)
    RETURN arctan2(x, sqrt(ONE - x * x))
END arcsin;


PROCEDURE arccos* (x: REAL): REAL;
BEGIN
    ASSERT(ABS(x) <= ONE)
    RETURN arctan2(sqrt(ONE - x * x), x)
END arccos;


PROCEDURE arctan* (x: REAL): REAL;
    RETURN arctan2(x, ONE)
END arctan;


PROCEDURE sinh* (x: REAL): REAL;
BEGIN
    x := exp(x)
    RETURN (x - ONE / x) * HALF
END sinh;


PROCEDURE cosh* (x: REAL): REAL;
BEGIN
    x := exp(x)
    RETURN (x + ONE / x) * HALF
END cosh;


PROCEDURE tanh* (x: REAL): REAL;
BEGIN
    IF x > 15.0 THEN
        x := ONE
    ELSIF x < -15.0 THEN
        x := -ONE
    ELSE
        x := exp(TWO * x);
        x := (x - ONE) / (x + ONE)
    END

    RETURN x
END tanh;


PROCEDURE arsinh* (x: REAL): REAL;
    RETURN ln(x + sqrt(x * x + ONE))
END arsinh;


PROCEDURE arcosh* (x: REAL): REAL;
BEGIN
    ASSERT(x >= ONE)
    RETURN ln(x + sqrt(x * x - ONE))
END arcosh;


PROCEDURE artanh* (x: REAL): REAL;
BEGIN
    ASSERT(ABS(x) < ONE)
    RETURN HALF * ln((ONE + x) / (ONE - x))
END artanh;


PROCEDURE sgn* (x: REAL): INTEGER;
VAR
    res: INTEGER;

BEGIN
    IF x > ZERO THEN
        res := 1
    ELSIF x < ZERO THEN
        res := -1
    ELSE
        res := 0
    END

    RETURN res
END sgn;


PROCEDURE fact* (n: INTEGER): REAL;
VAR
    res: REAL;

BEGIN
    res := ONE;
    WHILE n > 1 DO
        res := res * FLT(n);
        DEC(n)
    END

    RETURN res
END fact;


PROCEDURE DegToRad* (x: REAL): REAL;
    RETURN x * (pi / 180.0)
END DegToRad;


PROCEDURE RadToDeg* (x: REAL): REAL;
    RETURN x * (180.0 / pi)
END RadToDeg;


(* Return hypotenuse of triangle *)
PROCEDURE hypot* (x, y: REAL): REAL;
VAR
    a: REAL;

BEGIN
    x := ABS(x);
    y := ABS(y);
    IF x > y THEN
        a := x * sqrt(1.0 + sqrr(y / x))
    ELSE
        IF x > 0.0 THEN
            a := y * sqrt(1.0 + sqrr(x / y))
        ELSE
            a := y
        END
    END

    RETURN a
END hypot;


BEGIN
    large := 1.9;
    PACK(large, expoMax);
    miny := ONE / large;
    LnInfinity := ln(large);
    LnSmall    := ln(miny);
END Math.