library IEEE;
use IEEE.Std_Logic_1164.all;
use IEEE.Numeric_Std.all;
use IEEE.Math_Real.all;

package MokuSupport is
    type S16Array is array (natural range <>) of signed(15 downto 0);
    type S32Array is array (natural range <>) of signed(31 downto 0);
    type S25Array is array (natural range <>) of signed(24 downto 0);
    type S48Array is array (natural range <>) of signed(47 downto 0);

    type IntArray is array (natural range <>) of integer;

    constant ZERO_S16 : signed(15 downto 0) := (others => '0');

    type ControlRegister is record
        Value : std_logic_vector(31 downto 0);
        WriteStrobe : std_logic;
        ReadStrobe : std_logic;
    end record;

    type ControlRegisterArray is array (natural range <>) of ControlRegister;

    subtype StatusRegister is std_logic_vector(31 downto 0);

    type StatusRegisterArray is array (natural range <>) of StatusRegister;

    type LADCChannel is record
        Data : signed(17 downto 0);
        Valid : std_logic;
    end record;
    type LADCChannelArray is array (natural range <>) of LADCChannel;

    type LADCChannel20b is record
        Data : signed(19 downto 0);
        Valid : std_logic;
    end record;
    type LADCChannel20bArray is array (natural range <>) of LADCChannel20b;

    type AInChannel is record
        Data : signed(63 downto 0);  --actually concatenated signed values
        Valid : std_logic;
    end record;
    type AInChannelArray is array (natural range <>) of AInChannel;

    subtype QuadAInChannel is AInChannelArray(0 to 3);
    type QuadAInChannelArray is array (natural range <>) of QuadAInChannel;

    type AInChannelX1 is record
        Data : signed(15 downto 0);
        Valid : std_logic;
    end record;
    type AInChannelX1Array is array (natural range <>) of AInChannelX1;

    constant AIN_ZERO : AInChannel := (Data => (others => '0'), Valid => '0');

    type DataChannel is record
        Data : std_logic_vector(63 downto 0);
        Trigger : std_logic_vector(7 downto 0);
        Marker : std_logic;
        Meta : StatusRegisterArray(0 to 5);
        Valid : std_logic;
        Last : std_logic;
        Sync : std_logic;
    end record;
    type DataChannelArray is array (natural range <>) of DataChannel;

    constant DATA_CHANNEL_ZERO : DataChannel := (
            Data => (others => '0'),
            Trigger => (others => '0'),
            Marker => '0',
            Meta => (others => (others => '0')),
            Valid => '0',
            Last => '0',
            Sync => '0'
        );

    type URAMChannel is record
        Data : std_logic_vector(127 downto 0);
        Addr : std_logic_vector(22 downto 0);
        WriteEn : std_logic;
        Enable : std_logic;
    end record;
    type URAMChannelArray is array (natural range <>) of URAMChannel;

    function to_ain(T : AInChannelX1) return AInChannel;
    function to_ain(S : S16Array(0 to 3); V : std_logic) return AInChannel;
    function shift_right(A : AInChannel; Shift : integer) return AInChannel;
    function shift_left(A : AInChannel; Shift : integer) return AInChannel;
    function "+" (A, B : AInChannel) return AInChannel;
    function clip_val(X : AInChannel; MinVal, MaxVal : integer) return AInChannel;
    function sum_no_overflow(A : AInChannel; B : signed) return AInChannel;
    function to_s16x4array(A : AInChannel) return S16Array;
    function to_s32x4array(A : AInChannel) return S32Array;
    function to_status_array(A : ControlRegisterArray) return StatusRegisterArray;
    function sum_no_overflow(A : signed; B : signed) return signed;
    function sum_no_overflow(A : signed; B : integer) return signed;
    function sum_no_overflow(A : unsigned; B : signed) return unsigned;
    function sum_no_overflow(A : unsigned; B : integer) return unsigned;
    function sum_modulo(A : unsigned; B : unsigned; N : unsigned) return unsigned;
    function negate(A : signed) return signed;
    function clip(A : signed; NewLeft, NewRight : integer) return signed;
    function clip(A : unsigned; NewLeft, NewRight : integer) return unsigned;
    function clip_val(A : signed; MinVal : integer; MaxVal : integer) return signed;
    function grey_to_signed(X : std_logic_vector) return signed;
    function clog2(X : integer) return integer;
    function flog2(X : integer) return integer;
    function rlog2(X : integer) return integer;
    function or_reduce(X: std_logic_vector) return std_logic;
    function and_reduce(X: std_logic_vector) return std_logic;
    function all_bits_equal(X: std_logic_vector) return std_logic;
    function conditional(B : boolean; X, Y : integer) return integer;
    function mask_slv(LEN, ONES : integer) return std_logic_vector;
    function popcount(X : std_logic_vector) return integer;

    -- see doc/Rounding.md
    function zero_pad(X : signed; Size : integer) return signed;
    function zero_pad(X : unsigned; Size : integer) return unsigned;
    function sign_extend(X : signed; Size : integer) return signed;
    function sign_extend(X : unsigned; Size : integer) return unsigned;
    function discard_msbs(X : signed; V : std_logic; Size : integer) return signed;
    function discard_msbs(X : unsigned; V : std_logic; Size : integer) return unsigned;
    function truncate(X : signed; Size : integer) return signed;
    function truncate(X : unsigned; Size : integer) return unsigned;
    function round_half_up(X : signed; V : std_logic; Size : integer) return signed;
    function round_half_up(X : unsigned; V : std_logic; Size : integer) return unsigned;
    function round_half_even(X : signed; V : std_logic; Size : integer) return signed;
    function round_half_even(X : unsigned; V : std_logic; Size : integer) return unsigned;
    function truncate_dither(X : signed; V : std_logic; Size : integer; R : std_logic_vector) return signed;
    function truncate_dither(X : unsigned; V : std_logic; Size : integer; R : std_logic_vector) return unsigned;

    function to_s16array(S : signed) return S16Array;
    function to_s16array(A : AInChannelArray) return S16Array;
    function to_signed(A : S16Array) return signed;
    function to_ain_array(B : S16Array; V : std_logic) return AInChannelArray;

    component Interpolator
        generic (
            OUT_REG : integer range 0 to 1 := 0
        );
        port (
            Clk : in std_logic;
            Reset : in std_logic;
            A : in signed;  --Point A
            B : in signed; --Point B
            N : in unsigned;    --Distance from A to B to resolve Z
            Z : out signed  --Result
        );
    end component;

    component ScaleOffset
        generic (
            NORMAL_SHIFT : integer := 0;
            OFFSET_SHIFT : integer := 0;
            ROUNDING : boolean := true;
            IN_REG : integer range 0 to 1 := 0;
            OUT_REG : integer range 0 to 1 := 0;
            MID_REG : integer range 0 to 1 := 1
        );
        port (
            Clk : in std_logic;
            Reset : in std_logic;
            X : in signed;
            Scale : in signed;
            Offset : in signed;
            Z : out signed;
            Valid : in std_logic;
            OutValid : out std_logic
        );
    end component;

    component ScaleOffset2
        generic (
            NORMAL_SHIFT : integer := 0;
            OFFSET_SHIFT : integer := 0;
            ROUNDING : boolean := true;
            IN_REG : integer range 0 to 1 := 0;
            OUT_REG : integer range 0 to 1 := 0;
            MID_REG : integer range 0 to 1 := 1
        );
        port (
            Clk : in std_logic;
            Reset : in std_logic;
            X : in signed;
            Y : in signed;
            Scale : in signed;
            Offset : in signed;
            Z : out signed;
            Valid : in std_logic;
            OutValid : out std_logic
        );
    end component;

    component RegCrossing
        port (
            SrcClk : in std_logic;
            DstClk : in std_logic;
            RegIn : in ControlRegister;
            RegOut : out ControlRegister
        );
    end component;
end package;

package body MokuSupport is
    function to_ain(T : AInChannelX1) return AInChannel is
        variable A : AInChannel;
    begin
        -- TODO interpolate?
        A.Data(15 downto  0) := T.Data;
        A.Data(31 downto 16) := T.Data;
        A.Data(47 downto 32) := T.Data;
        A.Data(63 downto 48) := T.Data;
        A.Valid := T.Valid;
        return A;
    end function;

    function to_ain(S : S16Array(0 to 3); V : std_logic) return AInChannel is
        variable Z : AInChannel;
    begin
        Z.Data(15 downto  0) := S(0);
        Z.Data(31 downto 16) := S(1);
        Z.Data(47 downto 32) := S(2);
        Z.Data(63 downto 48) := S(3);
        Z.Valid := V;
        return Z;
    end function;

    function shift_right(A : AInChannel; Shift : integer) return AInChannel is
        variable X : AInChannel;
    begin
        X.Data(15 downto  0) := shift_right(A.Data(15 downto  0), Shift);
        X.Data(31 downto 16) := shift_right(A.Data(31 downto 16), Shift);
        X.Data(47 downto 32) := shift_right(A.Data(47 downto 32), Shift);
        X.Data(63 downto 48) := shift_right(A.Data(63 downto 48), Shift);
        X.Valid := A.Valid;
        return X;
    end function;

    function shift_left(A : AInChannel; Shift : integer) return AInChannel is
        variable X : AInChannel;
    begin
        X.Data(15 downto  0) := shift_left(A.Data(15 downto  0), Shift);
        X.Data(31 downto 16) := shift_left(A.Data(31 downto 16), Shift);
        X.Data(47 downto 32) := shift_left(A.Data(47 downto 32), Shift);
        X.Data(63 downto 48) := shift_left(A.Data(63 downto 48), Shift);
        X.Valid := A.Valid;
        return X;
    end function;

    function "+" (A, B : AInChannel) return AInChannel is
        variable Y : AInChannel;
    begin
        Y.Data(15 downto 0) := A.Data(15 downto 0) + B.Data(15 downto 0);
        Y.Data(31 downto 16) := A.Data(31 downto 16) + B.Data(31 downto 16);
        Y.Data(47 downto 32) := A.Data(47 downto 32) + B.Data(47 downto 32);
        Y.Data(63 downto 48) := A.Data(63 downto 48) + B.Data(63 downto 48);
        Y.Valid := A.Valid and B.Valid;
        return Y;
    end function;

    function clip_val(X : AInChannel; MinVal, MaxVal : integer) return AInChannel is
        variable Y : AInChannel;
    begin
        Y.Data(15 downto 0) := clip_val(X.Data(15 downto 0), MinVal, MaxVal);
        Y.Data(31 downto 16) := clip_val(X.Data(31 downto 16), MinVal, MaxVal);
        Y.Data(47 downto 32) := clip_val(X.Data(47 downto 32), MinVal, MaxVal);
        Y.Data(63 downto 48) := clip_val(X.Data(63 downto 48), MinVal, MaxVal);
        Y.Valid := X.Valid;
        return Y;
    end function;

    function sum_no_overflow(A : AInChannel; B : signed) return AInChannel is
        variable Y : AInChannel;
    begin
        Y.Data(15 downto 0) := sum_no_overflow(A.Data(15 downto 0), B);
        Y.Data(31 downto 16) := sum_no_overflow(A.Data(31 downto 16), B);
        Y.Data(47 downto 32) := sum_no_overflow(A.Data(47 downto 32), B);
        Y.Data(63 downto 48) := sum_no_overflow(A.Data(63 downto 48), B);
        Y.Valid := A.Valid;
        return Y;
    end function;

    function to_s16x4array(A : AInChannel) return S16Array is
        variable O : S16Array(0 to 3);
    begin
        O(0) := A.Data(15 downto  0);
        O(1) := A.Data(31 downto 16);
        O(2) := A.Data(47 downto 32);
        O(3) := A.Data(63 downto 48);
        return O;
    end function;

    function to_s32x4array(A : AInChannel) return S32Array is
        variable O : S32Array(0 to 3);
    begin
        O(0) := resize(A.Data(15 downto  0), 32);
        O(1) := resize(A.Data(31 downto 16), 32);
        O(2) := resize(A.Data(47 downto 32), 32);
        O(3) := resize(A.Data(63 downto 48), 32);
        return O;
    end function;

    function to_status_array(A : ControlRegisterArray) return StatusRegisterArray is
        variable X : StatusRegisterArray(A'range);
    begin
        for I in A'range loop
            X(I) := A(I).Value;
        end loop;
        return X;
    end function;

    function sum_no_overflow(A : signed; B : signed) return signed is
        variable Sum : signed(A'range);
        variable Overflow : boolean;
    begin
        assert A'length >= B'length report "A must be the same or greater length as B" severity FAILURE;
        Sum := A + B;
        --If A and B have a different sign, then no overflow.  Same signed operands should yeild the same signed result.
        Overflow := (A(A'left) = B(B'left)) and (A(A'left) /= Sum(Sum'left));
        if Overflow then
            if A(A'left) = '1' then
                return signed('1' & resize(unsigned'("00"), A'length-1));
            else
                return signed('0' & not resize(unsigned'("00"), A'length-1));
            end if;
        else
            return Sum;
        end if;
    end function;

    function sum_no_overflow(A : unsigned; B : signed) return unsigned is
        variable Sum : signed(A'left + 1 downto A'Right);
        variable Overflow : boolean;
    begin
        Sum := signed('0' & A) + B;
        --If A and B have a different sign, then no overflow.  Same signed operands should yeild the same signed result.
        Overflow := Sum(Sum'left) = '1';
        if Overflow then
            if B(B'left) = '1' then
                return resize(unsigned'("00"), A'length);
            else
                return not resize(unsigned'("00"), A'length);
            end if;
        else
            return unsigned(Sum(Sum'left-1 downto Sum'Right));
        end if;
    end function;

    function sum_no_overflow(A : signed; B : integer) return signed is
    begin
        return sum_no_overflow(A, to_signed(B, A'length));
    end function;

    function sum_no_overflow(A : unsigned; B : integer) return unsigned is
    begin
        return sum_no_overflow(A, to_signed(B, A'length));
    end function;

    -- (A + B) mod N
    function sum_modulo(A : unsigned; B : unsigned; N : unsigned) return unsigned is
        variable Sum : unsigned(A'left+1 downto A'right); --prevent modulo 2^A'length
    begin
        assert A'length >= B'length report "A must be the same or greater length as B" severity FAILURE;
        assert A'length >= N'length report "A must be the same or greater length as N" severity FAILURE;
        Sum := resize(A, Sum'length) + B;

        if Sum >= N then
            return resize(Sum - N, A'length);
        else
            return resize(Sum, A'length);
        end if;
    end function;

    function negate(A : signed) return signed is
    begin
        if A = -2**(A'left) then
            return signed('0' & not resize(unsigned'("00"), A'length-1));
        else
            return -A;
        end if;
    end function;

    -- the clip function resizes a signed input signal into a signed(NewLeft downto NewRight) signal.
    -- Additionally if the original signal's value is out of range of the output signal the output signal is clipped to its MAX/MIN
    function clip(A : signed; NewLeft, NewRight : integer) return signed is
        variable Upper : signed(A'left downto NewLeft);
        variable NewRange : integer;
    begin
        assert NewLeft <= A'left report "NewLeft out of range" severity FAILURE;
        assert NewRight >= A'right report "NewRight out of range" severity FAILURE;
        assert NewLeft > NewRight report "NewLeft must be greater than NewRight" severity FAILURE;

        NewRange := NewLeft - NewRight + 1;

        -- Sometimes we call clip without actually needing to clip
        if A'left = NewLeft and A'right = NewRight then
            return A;
        end if;

        Upper := A(A'left downto NewLeft);
        assert NewRange < A'length report "A must be the same or greater length as NewRange" severity FAILURE;

        if Upper = to_signed(-1, Upper'length) then
            return A(NewLeft downto NewRight);
        elsif Upper = to_signed(0, Upper'length) then
            return A(NewLeft downto NewRight);
        elsif Upper(Upper'left) = '1' then
          return signed('1' & resize(unsigned'("00"), NewRange - 1));
        else
            return signed('0' & not resize(unsigned'("00"), NewRange - 1));
        end if;
    end function;

    function clip(A : unsigned; NewLeft, NewRight : integer) return unsigned is
    begin
        assert NewLeft <= A'left report "NewLeft out of range" severity FAILURE;
        assert NewRight >= A'right report "NewRight out of range" severity FAILURE;
        assert NewLeft > NewRight report "NewLeft must be greater than NewRight" severity FAILURE;

        if or A(A'left downto NewLeft + 1) then
            return not to_unsigned(0, NewLeft - NewRight + 1);
        else
            return A(NewLeft downto NewRight);
        end if;
    end function;

    -- Clip A by value to Min/Max inclusive.  Return signed of the same size
    -- as A
    function clip_val(A : signed; MinVal : integer; MaxVal : integer) return signed is
        variable Z : signed(A'range);
    begin
        assert MinVal < MaxVal report "Min >= Max" severity WARNING;
        assert MinVal >= -2**(A'length - 1) report "Min out of range of A" severity WARNING;
        assert MaxVal <= 2**(A'length - 1) - 1 report "Max out of range of A" severity WARNING;

        if A < MinVal then
            Z := to_signed(MinVal, Z'length);
        elsif A > MaxVal then
            Z := to_signed(MaxVal, Z'length);
        else
            Z := A;
        end if;

        return Z;
    end function;

    function grey_to_signed(X : std_logic_vector) return signed is
        variable A : std_logic_vector(X'range);
    begin
        A(A'left) := X(A'left);

        for I in X'left-1 downto 0 loop
            A(I) := A(I+1) xor X(I);
        end loop;

        return signed(A);
    end function;

    function clog2(X : integer) return integer is
    begin
        return integer(ceil(log2(real(X))));
    end function;


    function flog2(X : integer) return integer is
    begin
        return integer(floor(log2(real(X))));
    end function;


    function rlog2(X : integer) return integer is
    begin
        return integer(round(log2(real(X))));
    end function;

    function or_reduce(X: std_logic_vector) return std_logic is
        variable Z : std_logic;
    begin
        Z := '0';

        for I in X'range loop
            Z := Z or X(I);
        end loop;

        return Z;
    end function;

    function and_reduce(X: std_logic_vector) return std_logic is
        variable Z : std_logic;
    begin
        Z := '1';

        for I in X'range loop
            Z := Z and X(I);
        end loop;

        return Z;
    end function;

    function all_bits_equal(X: std_logic_vector) return std_logic is
        variable Z : std_logic;
    begin
        Z := X(X'right);

        for I in X'range loop
            if Z /= X(I) then
                return '0';
            end if;
        end loop;

        return '1';
    end function;

    function conditional(B : boolean; X, Y : integer) return integer is
    begin
        if (B) then
            return X;
        else
            return Y;
        end if;
    end function;

    function mask_slv(LEN, ONES : integer) return std_logic_vector is
        variable A : std_logic_vector(LEN - 1 downto 0) := (others => '0');
    begin
        for I in 0 to ONES - 1 loop
            A := A(A'left - 1 downto 0) & '1';
        end loop;
        return A;
    end function;

    function popcount(X : std_logic_vector) return integer is
        variable Z : integer;
    begin
        Z := 0;

        for I in X'range loop
            if X(I) = '1' then
                Z := Z + 1;
            end if;
        end loop;

        return Z;
    end function;

    function zero_pad(X : signed; Size : integer) return signed is
    begin
        assert Size >= X'length report "Size must be greater than or equal to signal width" severity FAILURE;
        return shift_left(resize(X, Size), Size - X'length);
    end function;

    function zero_pad(X : unsigned; Size : integer) return unsigned is
    begin
        assert Size >= X'length report "Size must be greater than or equal to signal width" severity FAILURE;
        return shift_left(resize(X, Size), Size - X'length);
    end function;

    function sign_extend(X : signed; Size : integer) return signed is
    begin
        assert Size >= X'length report "Size must be greater than or equal to signal width" severity FAILURE;
        return resize(X, Size);
    end function;

    function sign_extend(X : unsigned; Size : integer) return unsigned is
    begin
        assert Size >= X'length report "Size must be greater than or equal to signal width" severity FAILURE;
        return resize(X, Size);
    end function;

    function discard_msbs(X : signed; V : std_logic; Size : integer) return signed is
    begin
        assert Size < X'length report "Size must be less than signal width" severity FAILURE;
        if V = '1' then
            assert all_bits_equal(std_logic_vector(X(X'left downto Size - 1))) = '1' report "Wrapping detected" severity FAILURE;
        end if;
        return resize(X, Size);
    end function;

    function discard_msbs(X : unsigned; V : std_logic; Size : integer) return unsigned is
    begin
        assert Size < X'length report "Size must be less than signal width" severity FAILURE;
        if V = '1' then
            assert or(std_logic_vector(X(X'left downto Size))) = '0' report "Wrapping detected" severity FAILURE;
        end if;
        return resize(X, Size);
    end function;

    function truncate(X : signed; Size : integer) return signed is
        variable D : integer;
    begin
        D := X'length - Size;
        assert Size > 0 report "Size must be greater than zero" severity FAILURE;
        assert D > 0 report "Size must be less than signal width" severity FAILURE;
        return resize(shift_right(X, D), Size);
    end function;

    function truncate(X : unsigned; Size : integer) return unsigned is
        variable D : integer;
    begin
        D := X'length - Size;
        assert Size > 0 report "Size must be greater than zero" severity FAILURE;
        assert D > 0 report "Size must be less than signal width" severity FAILURE;
        return resize(shift_right(X, D), Size);
    end function;

    function round_half_up(X : signed; V : std_logic; Size : integer) return signed is
        variable D : integer;
        variable R, MaxVal : signed(X'Range);
    begin
        D := X'length - Size;
        MaxVal(MaxVal'left) := '0';
        MaxVal(MaxVal'left-1 downto 0) := (others => '1');
        assert Size > 0 report "Size must be greater than zero" severity FAILURE;
        assert D > 0 report "Size must be less than signal width" severity FAILURE;
        assert D > 1 report "Cannot round half-up when truncating one bit, use round_half_even instead" severity FAILURE;
        R := shift_left(to_signed(1, R'length), D - 1);  -- equivalent to 2**(D-1), but works for D > 32
        if V = '1' then
            assert X <= MaxVal - R report "Rounding a value within half an LSB of maximum will cause overflow" severity FAILURE;
        end if;
        return resize(shift_right(X + R, D), Size);
    end function;

    function round_half_up(X : unsigned; V : std_logic; Size : integer) return unsigned is
        variable D : integer;
        variable R, MaxVal : unsigned(X'Range);
    begin
        D := X'length - Size;
        MaxVal(MaxVal'left) := '0';
        MaxVal(MaxVal'left-1 downto 0) := (others => '1');
        assert Size > 0 report "Size must be greater than zero" severity FAILURE;
        assert D > 0 report "Size must be less than signal width" severity FAILURE;
        assert D > 1 report "Cannot round half-up when truncating one bit, use round_half_even instead" severity FAILURE;
        R := shift_left(to_unsigned(1, R'length), D - 1);  -- equivalent to 2**(D-1), but works for D > 32
        if V = '1' then
            assert X <= MaxVal - R report "Rounding a value within half an LSB of maximum will cause overflow" severity FAILURE;
        end if;
        return resize(shift_right(X + R, D), Size);
    end function;

    function round_half_even(X : signed; V : std_logic; Size : integer) return signed is
        variable D : integer;
        variable R, MaxVal : signed(X'Range);
    begin
        D := X'length - Size;
        MaxVal := ('0', others => '1');
        assert Size > 0 report "Size must be greater than zero" severity FAILURE;
        assert D > 0 report "Size must be less than signal width" severity FAILURE;
        R := shift_left(to_signed(1, R'length), D - 1);  -- equivalent to 2**(D-1), but works for D > 32
        if V = '1' then
            assert X <= MaxVal - R report "Rounding a value within half an LSB of maximum will cause overflow" severity FAILURE;
        end if;
        return resize(shift_right(X + R + X(D) - 1, D), Size);
    end function;

    function round_half_even(X : unsigned; V : std_logic; Size : integer) return unsigned is
        variable D : integer;
        variable R, MaxVal : unsigned(X'Range);
    begin
        D := X'length - Size;
        MaxVal := (others => '1');
        assert Size > 0 report "Size must be greater than zero" severity FAILURE;
        assert D > 0 report "Size must be less than signal width" severity FAILURE;
        R := shift_left(to_unsigned(1, R'length), D - 1);  -- equivalent to 2**(D-1), but works for D > 32
        if V = '1' then
            assert X <= MaxVal - R report "Rounding a value within half an LSB of maximum will cause overflow" severity FAILURE;
        end if;
        return resize(shift_right(X + R + X(D) - 1, D), Size);
    end function;

    function truncate_dither(X : signed; V : std_logic; Size : integer; R : std_logic_vector) return signed is
        variable D : integer;
        variable MaxVal, MaxDither : signed(X'Range);
    begin
        D := X'length - Size;
        MaxVal(MaxVal'left) := '0';
        MaxVal(MaxVal'left-1 downto 0) := (others => '1');
        MaxDither(MaxDither'left downto D) := (others => '0');
        MaxDither(D-1 downto 0) := (others => '1');
        assert Size > 0 report "Size must be greater than zero" severity FAILURE;
        assert D > 0 report "Size must be less than signal width" severity FAILURE;
        assert R'length = D report "Number of random bits provided must match number of bits being truncated" severity FAILURE;
        if V = '1' then
            assert X <= MaxVal - MaxDither report "Rounding a value within largest possible dither value of maximum may cause overflow" severity FAILURE;
        end if;
        return resize(shift_right(X + signed('0' & R), D), Size);
    end function;

    function truncate_dither(X : unsigned; V : std_logic; Size : integer; R : std_logic_vector) return unsigned is
        variable D : integer;
        variable MaxVal, MaxDither : unsigned(X'Range);
    begin
        D := X'length - Size;
        MaxVal(MaxVal'left) := '0';
        MaxVal(MaxVal'left-1 downto 0) := (others => '1');
        MaxDither(MaxDither'left downto D) := (others => '0');
        MaxDither(D-1 downto 0) := (others => '1');
        assert Size > 0 report "Size must be greater than zero" severity FAILURE;
        assert D > 0 report "Size must be less than signal width" severity FAILURE;
        assert R'length = D report "Number of random bits provided must match number of bits being truncated" severity FAILURE;
        if V = '1' then
            assert X <= MaxVal - MaxDither report "Rounding a value within largest possible dither value of maximum may cause overflow" severity FAILURE;
        end if;
        return resize(shift_right(X + unsigned(R), D), Size);
    end function;

    function to_s16array(S : signed) return S16Array is
        variable A : S16Array(0 to S'length / 16 - 1);
        variable B : signed(S'length - 1 downto 0) := S;  -- rebase to zero
    begin
        for Idx in A'range loop
            A(Idx) := B(16 * Idx + 15 downto 16 * Idx);
        end loop;

        return A;
    end function;

    function to_s16array(A : AInChannelArray) return S16Array is
        variable X : AInChannelArray(0 to A'length - 1) := A;  --rebase range to 0
        variable B : S16Array(0 to A'length * 4 - 1);
    begin
        for I in 0 to 3 loop
            for J in X'range loop
                B(X'length * I + J) := X(J).Data(16 * I + 15 downto 16 * I);
            end loop;
        end loop;

        return B;
    end function;

    function to_signed(A : S16Array) return signed is
        variable S : signed(A'length * 16 - 1 downto 0);
    begin
        for Idx in A'range loop
            S(16 * Idx + 15 downto 16 * Idx) := A(Idx);
        end loop;

        return S;
    end function;

    function to_ain_array(B : S16Array; V : std_logic) return AInChannelArray is
        variable X : S16Array(0 to B'length - 1) := B;  --rebase range to 0
        variable A : AInChannelArray(0 to B'length / 4 - 1);
    begin
        for I in 0 to 3 loop
            for J in A'range loop
                A(J).Data(16 * I + 15 downto 16 * I) := X(A'length * I + J);
            end loop;
        end loop;

        for I in A'range loop
            A(I).Valid := V;
        end loop;

        return A;
    end function;
end package body;

library IEEE;
use IEEE.Std_Logic_1164.all;
use IEEE.Numeric_Std.all;

library UNISIM;
use UNISIM.vcomponents.all;

--Linearly interpolate from A to B a distance of 2^17 / N
--One Clock Cycle Delay
entity Interpolator is
    generic (
        OUT_REG : integer range 0 to 1 := 0
    );
    port (
        Clk : in std_logic;
        Reset : in std_logic;
        A : in signed;  --Point A
        B : in signed;  --Point B
        N : in unsigned;  --Distance from A to B to resolve Z
        Z : out signed  --Result
    );
end entity;

architecture Behavioural of Interpolator is
    signal DSPA : std_logic_vector(29 downto 0);
    signal DSPB : std_logic_vector(17 downto 0);
    signal DSPC : std_logic_vector(47 downto 0);
    signal DSPD : std_logic_vector(24 downto 0);
    signal DSPP : std_logic_vector(47 downto 0);
begin
    assert N'length <= 17 report "N vector to large" severity FAILURE;
    assert A'length <= 25 report "A vector to large" severity FAILURE;
    assert A'length = B'length report "A and B need to be the same size" severity FAILURE;
    assert Z'length = A'length report "Z needs to be same length as A and B" severity FAILURE;

    -- Z = (B - A) * N + A
    DSPA <= std_logic_vector(resize(A, DSPA'length));
    DSPD <= std_logic_vector(resize(B, DSPD'length));
    DSPB <= std_logic_vector(signed('0' & N));
    DSPC <= std_logic_vector(shift_left(resize(A, DSPC'length), N'length));

    -- (D-A) * B + C
    DSP: DSP48E1
        generic map (
            AREG => 0,
            DREG => 0,
            BREG => 1,
            CREG => 1,
            ADREG => 1,
            MREG => 0,
            PREG => OUT_REG,
            USE_DPORT => True,
            ACASCREG => 0,
            BCASCREG => 1,
            ALUMODEREG => 0,
            INMODEREG => 0,
            OPMODEREG => 0
        )
        port map (
            CLK => Clk,
            A => DSPA,
            B => DSPB,
            C => DSPC,
            D => DSPD,
            ACIN => (others => '0'),
            ACOUT => open,
            ALUMODE => "0000",
            BCIN => (others => '0'),
            BCOUT => open,
            CARRYCASCIN => '0',
            CARRYCASCOUT => open,
            CARRYIN => '0',
            CARRYINSEL => (others => '0'),
            CARRYOUT => open,
            CEA1 => '0',
            CEA2 => '0',
            CEAD => '1',
            CEALUMODE => '0',
            CEB1 => '0',
            CEB2 => '1',
            CEC => '1',
            CECARRYIN => '0',
            CECTRL => '0',
            CED => '0',
            CEINMODE => '0',
            CEM => '0',
            CEP => '1',
            INMODE => "01100",
            MULTSIGNIN => '0',
            MULTSIGNOUT => open,
            OPMODE => "0110101",
            OVERFLOW => open,
            P => DSPP,
            PATTERNBDETECT => open,
            PATTERNDETECT => open,
            PCIN => (others => '0'),
            PCOUT => open,
            RSTA => '0',
            RSTALLCARRYIN => '0',
            RSTALUMODE => '0',
            RSTB => Reset,
            RSTC => Reset,
            RSTCTRL => '0',
            RSTD => Reset,
            RSTINMODE => '0',
            RSTM => '0',
            RSTP => '0',
            UNDERFLOW => open
        );
    Z <= resize(shift_right(signed(DSPP) + 2**(N'length-1), N'length), Z'length);

end architecture;

library IEEE;
use IEEE.Std_Logic_1164.all;
use IEEE.Numeric_Std.all;

use WORK.MokuSupport.mask_slv;

library UNISIM;
use UNISIM.vcomponents.all;

--Z = X * Scale + Offset
--Clips Z to min/max (prevents over/underflow)
--Includes rounding
--One Clock Cycle Delay
entity ScaleOffset2 is
    generic (
        --Normalize Scale < 2^NORMAL_SHIFT
        NORMAL_SHIFT : integer := 0;
        --Offset shift with respect to X
        OFFSET_SHIFT : integer := 0;
        --Enable Rounding Bit
        ROUNDING : boolean := true;
        IN_REG : integer range 0 to 1 := 0;
        OUT_REG : integer range 0 to 1 := 0;
        MID_REG : integer range 0 to 1 := 1
    );
    port (
        Clk : in std_logic;
        Reset : in std_logic;
        X : in signed;
        Y : in signed;
        Scale : in signed;
        Offset : in signed;
        Z : out signed;
        Valid : in std_logic;  --Will still run with fixed delay without Valid
        OutValid : out std_logic  --Just convenient indicator
    );
end entity;

architecture Behavioural of ScaleOffset2 is
    constant MASK_MSBS : integer := Z'length + Scale'length - 2 - NORMAL_SHIFT;
    constant MASK : bit_vector(47 downto 0) := to_bitvector(mask_slv(48, MASK_MSBS));
    signal DSPA : std_logic_vector(29 downto 0);
    signal DSPB : std_logic_vector(17 downto 0);
    signal DSPC : std_logic_vector(47 downto 0);
    signal DSPD : std_logic_vector(24 downto 0);
    signal DSPP : std_logic_vector(47 downto 0);
    signal PD, PBD : std_logic;
    signal RoundBit : integer;
    signal CEM, CEP, CEC, CED, CEB2, CEA2 : std_logic;
begin
    assert X'length <= 25 report "X vector too large" severity FAILURE;
    assert Y'length <= 25 report "Y vector too large" severity FAILURE;
    assert Scale'length <= 18 report "Scale vector too large" severity FAILURE;
    assert Offset'length < 48-Scale'length report " Offset vector too large" severity FAILURE;
    assert MID_REG + IN_REG < 2 report "can't use IN_REG and MID_REG at the same time" severity FAILURE;

    RoundBit <= 2**(Scale'length - NORMAL_SHIFT - 2) when ROUNDING else 0;
    -- Z = Scale * (X + Y) + (Offset * 2^OFFSET_SHIFT)
    DSPA <= std_logic_vector(resize(X, DSPA'length));
    DSPB <= std_logic_vector(resize(Scale, DSPB'length));
    DSPC <= std_logic_vector(shift_left(resize(Offset, DSPC'length), Scale'left - NORMAL_SHIFT + OFFSET_SHIFT) + RoundBit);
    DSPD <= std_logic_vector(resize(Y, DSPD'length));

    CEM <= Valid when MID_REG = 1 else '0';
    CEP <= Valid when OUT_REG = 1 else '0';
    CEC <= Valid when MID_REG + IN_REG /= 0 else '0';
    CED <= Valid when IN_REG = 1 else '0';
    CEB2 <= Valid when IN_REG = 1 else '0';
    CEA2 <= Valid when IN_REG = 1 else '0';

    -- B * A + C
    DSP: DSP48E1
        generic map (
            AREG => IN_REG,
            DREG => IN_REG,
            BREG => IN_REG,
            CREG => MID_REG + IN_REG,
            ADREG => 0,
            MREG => MID_REG,
            PREG => OUT_REG,
            USE_DPORT => True,
            ACASCREG => IN_REG,
            BCASCREG => IN_REG,
            ALUMODEREG => 0,
            INMODEREG => 0,
            OPMODEREG => 0,
            PATTERN => x"000000000000",
            MASK => MASK,
            USE_PATTERN_DETECT => "PATDET"
        )
        port map (
            CLK => Clk,
            A => DSPA,
            B => DSPB,
            C => DSPC,
            D => DSPD,
            ACIN => (others => '0'),
            ACOUT => open,
            ALUMODE => "0000",
            BCIN => (others => '0'),
            BCOUT => open,
            CARRYCASCIN => '0',
            CARRYCASCOUT => open,
            CARRYIN => '0',
            CARRYINSEL => (others => '0'),
            CARRYOUT => open,
            CEA1 => '0',
            CEA2 => CEA2,
            CEAD => '0',
            CEALUMODE => '0',
            CEB1 => '0',
            CEB2 => CEB2,
            CEC => CEC,
            CECARRYIN => '0',
            CECTRL => '0',
            CED => CED,
            CEINMODE => '0',
            CEM => CEM,
            CEP => CEP,
            INMODE => "00100",
            MULTSIGNIN => '0',
            MULTSIGNOUT => open,
            OPMODE => "0110101",
            OVERFLOW => open,
            P => DSPP,
            PATTERNBDETECT => PBD,
            PATTERNDETECT => PD,
            PCIN => (others => '0'),
            PCOUT => open,
            RSTA => '0',
            RSTALLCARRYIN => '0',
            RSTALUMODE => '0',
            RSTB => '0',
            RSTC => Reset,
            RSTCTRL => '0',
            RSTD => '0',
            RSTINMODE => '0',
            RSTM => Reset,
            RSTP => '0',
            UNDERFLOW => open
        );

    process(PD, PBD, DSPP) is
    begin
        if PD = '0' and PBD = '0' then
            if DSPP(DSPP'left) = '0' then
                Z(Z'left) <= '0';
                Z(Z'left - 1 downto 0) <= (others => '1');
            else
                Z(Z'left) <= '1';
                Z(Z'left - 1 downto 0) <= (others => '0');
            end if;
        else
            --Rounding bit was added before DSP
            Z <= resize(shift_right(signed(DSPP), Scale'length-NORMAL_SHIFT - 1), Z'length);
        end if;
    end process;

    process(Clk) is
        variable Dly : std_logic;
    begin
        if rising_edge(Clk) then
            if Reset = '1' then
                OutValid <= '0';
                Dly := '0';
            else
                if IN_REG + OUT_REG = 2 then
                    OutValid <= Dly;
                else
                    OutValid <= Valid;
                end if;
                Dly := Valid;
            end if;
        end if;
    end process;
end architecture;

library IEEE;
use IEEE.Std_Logic_1164.all;
use IEEE.Numeric_Std.all;

library UNISIM;
use UNISIM.vcomponents.all;

entity ScaleOffset is
    generic (
        --Normalize Scale < 2^NORMAL_SHIFT
        NORMAL_SHIFT : integer := 0;
        --Offset shift with respect to X
        OFFSET_SHIFT : integer := 0;
        --Enable Rounding Bit
        ROUNDING : boolean := true;
        IN_REG : integer range 0 to 1 := 0;
        OUT_REG : integer range 0 to 1 := 0;
        MID_REG : integer range 0 to 1 := 1
    );
    port (
        Clk : in std_logic;
        Reset : in std_logic;
        X : in signed;
        Scale : in signed;
        Offset : in signed;
        Z : out signed;
        Valid : in std_logic;  --Will still run with fixed delay without Valid
        OutValid : out std_logic  --Just convenient indicator
    );
end entity;

architecture Behavioural of ScaleOffset is
    constant ZERO : signed(24 downto 0) := to_signed(0, 25);
begin
    SO: entity WORK.ScaleOffset2
        generic map (
            NORMAL_SHIFT => NORMAL_SHIFT,
            OFFSET_SHIFT => OFFSET_SHIFT,
            ROUNDING => ROUNDING,
            IN_REG => IN_REG,
            OUT_REG => OUT_REG,
            MID_REG => MID_REG
        )
        port map (
            Clk => Clk,
            Reset => Reset,
            X => X,
            Y => ZERO,
            Scale => Scale,
            Offset => Offset,
            Z => Z,
            Valid => Valid,
            OutValid => OutValid
        );
end architecture;
