Module MathFunc

    Public Function Deg2Rad(ByVal deg As Double)
        Const PI As Double = 3.14159265
        Return deg * PI / 180
    End Function

    Public Function Rad2Deg(ByVal rad As Double)
        Const PI As Double = 3.14159265
        Return rad * 180 / PI
    End Function

    Public Function ToFloat(ByRef buff() As Byte, ByRef offset As Integer) As Single
        offset += 4
        Return BitConverter.ToSingle(buff, offset - 4)
    End Function

    Public Function FromFixedPointToSigned(ByRef buff() As Byte, ByRef offset As Integer, ByVal X As Integer, ByVal Y As Integer) As Single
        ' X = integer part
        ' Y = fractional part
        Dim size As Integer = CInt((X + Y) / 8)
        offset += size
        Dim val As Integer '= (CInt(buff(offset - 1)) << 8) Or _
        ' buff(offset - 2)
        For i As Integer = 0 To size - 1
            val = val + (CInt(buff(offset - (size - i))) << (i * 8))
        Next

        Dim intPart As Single = val >> Y
        Dim fractPart As Single = val - (intPart << Y)
        If intPart > 2 ^ (X - 1) Then intPart = -(2 ^ X) + intPart
        Return intPart + fractPart / (2 ^ Y)
    End Function

    Public Function FromSingleToFixedPointConst(ByVal val As Single, ByVal X As Integer, ByVal Y As Integer) As Integer
        Dim maxVal As Single = (2 ^ (X - 1) - 1 / 2 ^ Y) - 1
        If val > maxVal Then val = maxVal
        If val < -maxVal Then val = -maxVal

        '((signed short)((int<<10) + ((fract + 0.00048828125)*1024)))
        '((A<<Y) + ((B +1/(2^(Y+1)))*2^Y))
        Dim fract As Single = Math.Abs(val Mod 1)
        Dim intPart As Integer = Fix(val)
        If val < 0 Then
            intPart = 2 ^ X + intPart
            Return (intPart << Y) - ((fract + 1 / (2 ^ (Y + 1))) * 2 ^ Y)
        Else
            Return (intPart << Y) + ((fract + 1 / (2 ^ (Y + 1))) * 2 ^ Y)
        End If
    End Function


    Public Function ToWORD(ByRef buff() As Byte, ByRef offset As Integer) As Integer
        offset += 2
        Return (CInt(buff(offset - 1)) << 8) Or buff(offset - 2)
    End Function


    Public Function ToDWORD(ByRef buff() As Byte, ByRef offset As Integer) As Integer
        offset += 4
        Return (CInt(buff(offset - 1)) << 24) Or _
                (CInt(buff(offset - 2)) << 16) Or _
                (CInt(buff(offset - 3)) << 8) Or _
                            buff(offset - 4)
    End Function

    Public Function WORDToBytes(ByVal i As Integer) As Byte()
        Dim data(1) As Byte
        data(0) = i And &HFF
        data(1) = i >> 8
        Return data
    End Function

    Public Function DWORDToBytes(ByVal i As Integer) As Byte()
        Dim data(3) As Byte
        data(0) = i And &HFF
        data(1) = (i >> 8) And &HFF
        data(2) = (i >> 16) And &HFF
        data(3) = (i >> 24) And &HFF
        Return data
    End Function

    Public Function ToSignedShort(ByRef buff() As Byte, ByRef offset As Integer) As Integer
        offset += 2
        Dim i As Integer = (CInt(buff(offset - 1)) << 8) Or buff(offset - 2)
        If i < 32768 Then Return i
        Return -65536 + i
    End Function

    Public Function ToBYTE(ByRef buff() As Byte, ByRef offset As Integer) As Integer
        offset += 1
        Return buff(offset - 1)
    End Function

    Public Sub CopyWORDToByteArray(ByVal i As Integer, ByRef b() As Byte, ByRef offset As Integer)
        b(offset) = i And &HFF
        b(offset + 1) = i >> 8
        offset += 2
    End Sub

    Public Sub CopyDWORDToByteArray(ByVal i As Integer, ByRef b() As Byte, ByRef offset As Integer)
        b(offset) = i And &HFF
        b(offset + 1) = (i >> 8) And &HFF
        b(offset + 2) = (i >> 16) And &HFF
        b(offset + 3) = (i >> 24) And &HFF
        offset += 4
    End Sub
End Module
