(*
    BSD 2-Clause License

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

MODULE FPU;


CONST

    INF  = 07F800000H;
    NINF = 0FF800000H;
    NAN  = 07FC00000H;


PROCEDURE div2 (b, a: INTEGER): INTEGER;
VAR
    n, e, r, s: INTEGER;

BEGIN
    s := ORD(BITS(a) / BITS(b) - {0..30});
    e := (a DIV 800000H) MOD 256 - (b DIV 800000H) MOD 256 + 127;

    a := a MOD 800000H + 800000H;
    b := b MOD 800000H + 800000H;

    n := 800000H;
    r := 0;

    IF a < b THEN
        a := a * 2;
        DEC(e)
    END;

    WHILE (a > 0) & (n > 0) DO
        IF a >= b THEN
            INC(r, n);
            DEC(a, b)
        END;
        a := a * 2;
        n := n DIV 2
    END;

    IF e <= 0 THEN
        e := 0;
        r := 800000H;
        s := 0
    ELSIF e >= 255 THEN
        e := 255;
        r := 800000H
    END

    RETURN (r - 800000H) + e * 800000H + s
END div2;


PROCEDURE mul2 (b, a: INTEGER): INTEGER;
VAR
    e, r, s: INTEGER;

BEGIN
    s := ORD(BITS(a) / BITS(b) - {0..30});
    e := (a DIV 800000H) MOD 256 + (b DIV 800000H) MOD 256 - 127;

    a := a MOD 800000H + 800000H;
    b := b MOD 800000H + 800000H;

    r := a * (b MOD 256);
    b := b DIV 256;
    r := LSR(r, 8);

    INC(r, a * (b MOD 256));
    b := b DIV 256;
    r := LSR(r, 8);

    INC(r, a * (b MOD 256));
    r := LSR(r, 7);

    IF r >= 1000000H THEN
        r := r DIV 2;
        INC(e)
    END;

    IF e <= 0 THEN
        e := 0;
        r := 800000H;
        s := 0
    ELSIF e >= 255 THEN
        e := 255;
        r := 800000H
    END

    RETURN (r - 800000H) + e * 800000H + s
END mul2;


PROCEDURE add2 (b, a: INTEGER): INTEGER;
VAR
    ea, eb, e, d, r: INTEGER;

BEGIN
    ea := (a DIV 800000H) MOD 256;
    eb := (b DIV 800000H) MOD 256;
    d  := ea - eb;

    a := a MOD 800000H + 800000H;
    b := b MOD 800000H + 800000H;

    IF d > 0 THEN
        IF d < 24 THEN
            b := LSR(b, d)
        ELSE
            b := 0
        END;
        e := ea
    ELSIF d < 0 THEN
        IF d > -24 THEN
            a := LSR(a, -d)
        ELSE
            a := 0
        END;
        e := eb
    ELSE
        e := ea
    END;

    r := a + b;

    IF r >= 1000000H THEN
        r := r DIV 2;
        INC(e)
    END;

    IF e >= 255 THEN
        e := 255;
        r := 800000H
    END

    RETURN (r - 800000H) + e * 800000H
END add2;


PROCEDURE sub2 (b, a: INTEGER): INTEGER;
VAR
    ea, eb, e, d, r, s: INTEGER;

BEGIN
    ea := (a DIV 800000H) MOD 256;
    eb := (b DIV 800000H) MOD 256;

    a := a MOD 800000H + 800000H;
    b := b MOD 800000H + 800000H;

    d := ea - eb;

    IF (d > 0) OR (d = 0) & (a >= b) THEN
        s := 0
    ELSE
        ea := eb;
        d := -d;
        r := a;
        a := b;
        b := r;
        s := 80000000H
    END;

    e := ea;

    IF d > 0 THEN
        IF d < 24 THEN
            b := LSR(b, d)
        ELSE
            b := 0
        END
    END;

    r := a - b;

    IF r = 0 THEN
        e := 0;
        r := 800000H;
        s := 0
    ELSE
        WHILE r < 800000H DO
            r := r * 2;
            DEC(e)
        END
    END;

    IF e <= 0 THEN
        e := 0;
        r := 800000H;
        s := 0
    END

    RETURN (r - 800000H) + e * 800000H + s
END sub2;


PROCEDURE zero (VAR x: INTEGER);
BEGIN
    IF BITS(x) * {23..30} = {} THEN
        x := 0
    END
END zero;


PROCEDURE isNaN (a: INTEGER): BOOLEAN;
    RETURN (a > INF) OR (a < 0) & (a > NINF)
END isNaN;


PROCEDURE isInf (a: INTEGER): BOOLEAN;
    RETURN (a = INF) OR (a = NINF)
END isInf;


PROCEDURE isNormal (a: INTEGER): BOOLEAN;
    RETURN (BITS(a) * {23..30} # {23..30}) & (BITS(a) * {23..30} # {})
END isNormal;


PROCEDURE add* (b, a: INTEGER): INTEGER;
VAR
    r: INTEGER;

BEGIN
    zero(a); zero(b);

    IF isNormal(a) & isNormal(b) THEN

        IF (a > 0) & (b > 0) THEN
            r := add2(b, a)
        ELSIF (a < 0) & (b < 0) THEN
            r := add2(b, a) + 80000000H
        ELSIF (a > 0) & (b < 0) THEN
            r := sub2(b, a)
        ELSIF (a < 0) & (b > 0) THEN
            r := sub2(a, b)
        END

    ELSIF isNaN(a) OR isNaN(b) THEN
        r := NAN
    ELSIF isInf(a) & isInf(b) THEN
        IF a = b THEN
            r := a
        ELSE
            r := NAN
        END
    ELSIF isInf(a) THEN
        r := a
    ELSIF isInf(b) THEN
        r := b
    ELSIF a = 0 THEN
        r := b
    ELSIF b = 0 THEN
        r := a
    END

    RETURN r
END add;


PROCEDURE sub* (b, a: INTEGER): INTEGER;
VAR
    r: INTEGER;

BEGIN
    zero(a); zero(b);

    IF isNormal(a) & isNormal(b) THEN

        IF (a > 0) & (b > 0) THEN
            r := sub2(b, a)
        ELSIF (a < 0) & (b < 0) THEN
            r := sub2(a, b)
        ELSIF (a > 0) & (b < 0) THEN
            r := add2(b, a)
        ELSIF (a < 0) & (b > 0) THEN
            r := add2(b, a) + 80000000H
        END

    ELSIF isNaN(a) OR isNaN(b) THEN
        r := NAN
    ELSIF isInf(a) & isInf(b) THEN
        IF a # b THEN
            r := a
        ELSE
            r := NAN
        END
    ELSIF isInf(a) THEN
        r := a
    ELSIF isInf(b) THEN
        r := INF + ORD(BITS(b) / {31} - {0..30})
    ELSIF (a = 0) & (b = 0) THEN
        r := 0
    ELSIF a = 0 THEN
        r := ORD(BITS(b) / {31})
    ELSIF b = 0 THEN
        r := a
    END

    RETURN r
END sub;


PROCEDURE mul* (b, a: INTEGER): INTEGER;
VAR
    r: INTEGER;

BEGIN
    zero(a); zero(b);

    IF isNormal(a) & isNormal(b) THEN
        r := mul2(b, a)
    ELSIF isNaN(a) OR isNaN(b) THEN
        r := NAN
    ELSIF (isInf(a) & (b = 0)) OR (isInf(b) & (a = 0)) THEN
        r := NAN
    ELSIF isInf(a) OR isInf(b) THEN
        r := INF + ORD(BITS(a) / BITS(b) - {0..30})
    ELSIF (a = 0) OR (b = 0) THEN
        r := 0
    END

    RETURN r
END mul;


PROCEDURE _div* (b, a: INTEGER): INTEGER;
VAR
    r: INTEGER;

BEGIN
    zero(a); zero(b);

    IF isNormal(a) & isNormal(b) THEN
        r := div2(b, a)
    ELSIF isNaN(a) OR isNaN(b) THEN
        r := NAN
    ELSIF isInf(a) & isInf(b) THEN
        r := NAN
    ELSIF isInf(a) THEN
        r := INF + ORD(BITS(a) / BITS(b) - {0..30})
    ELSIF isInf(b) THEN
        r := 0
    ELSIF a = 0 THEN
        IF b = 0 THEN
            r := NAN
        ELSE
            r := 0
        END
    ELSIF b = 0 THEN
        IF a > 0 THEN
            r := INF
        ELSE
            r := NINF
        END
    END

    RETURN r
END _div;


PROCEDURE cmp* (op, b, a: INTEGER): BOOLEAN;
VAR
    res: BOOLEAN;

BEGIN
    zero(a); zero(b);

    IF isNaN(a) OR isNaN(b) THEN
        res := op = 1
    ELSIF (a < 0) & (b < 0) THEN
        CASE op OF
        |0: res := a = b
        |1: res := a # b
        |2: res := a > b
        |3: res := a >= b
        |4: res := a < b
        |5: res := a <= b
        END
    ELSE
        CASE op OF
        |0: res := a = b
        |1: res := a # b
        |2: res := a < b
        |3: res := a <= b
        |4: res := a > b
        |5: res := a >= b
        END
    END

    RETURN res
END cmp;


PROCEDURE flt* (x: INTEGER): INTEGER;
VAR
    n, y, r, s: INTEGER;

BEGIN
    IF x = 0 THEN
        s := 0;
        r := 800000H;
        n := -126
    ELSIF x = 80000000H THEN
        s := 80000000H;
        r := 800000H;
        n := 32
    ELSE
        IF x < 0 THEN
            s := 80000000H
        ELSE
            s := 0
        END;
        n := 0;
        y := ABS(x);
        r := y;
        WHILE y > 0 DO
            y := y DIV 2;
            INC(n)
        END;
        IF n > 24 THEN
            r := LSR(r, n - 24)
        ELSE
            r := LSL(r, 24 - n)
        END
    END

    RETURN (r - 800000H) + (n + 126) * 800000H + s
END flt;


PROCEDURE floor* (x: INTEGER): INTEGER;
VAR
    r, e: INTEGER;

BEGIN
    zero(x);

    e := (x DIV 800000H) MOD 256 - 127;
    r := x MOD 800000H + 800000H;

    IF (0 <= e) & (e <= 22) THEN
        r := LSR(r, 23 - e) + ORD((x < 0) & (LSL(r, e + 9) # 0))
    ELSIF (23 <= e) & (e <= 54) THEN
        r := LSL(r, e - 23)
    ELSIF (e < 0) & (x < 0) THEN
        r := 1
    ELSE
        r := 0
    END;

    IF x < 0 THEN
        r := -r
    END

    RETURN r
END floor;


END FPU.