def calculate_crc(message):
    """
    Calculate the CRC of a message.
    :param bytearray message: Where each index is a byte
    """
    # Code converted from https://github.com/hazelnusse/crc7/blob/master/crc7.cc by devoh747
    # With permission from Dale Lukas Peterson <hazelnusse@gmail.com>
    # 8/6/2019

    crc_table = bytearray(256)

    crc_poly = const(0x89)  # the value of our CRC-7 polynomial

    # generate a table value for all 256 possible byte values
    for i in range(256):
        if (i & 0x80):
            crc_table[i] = i ^ crc_poly
        else:
            crc_table[i] = i
        for _ in range(1, 8):
            crc_table[i] = crc_table[i] << 1
            if (crc_table[i] & 0x80):
                crc_table[i] = crc_table[i] ^ crc_poly

    crc = 0
    # All messages in _cmd are 5 bytes including the cmd.. The 6th byte is the crc value.
    for i in range(0, 5):
        crc = crc_table[(crc << 1) ^ message[i]]

    return ((crc << 1) | 1)

def _cmd(cmd, arg=0, crc=0):

    buf = bytearray(6)
    buf[0] = 0x40 | cmd
    buf[1] = (arg >> 24) & 0xff
    buf[2] = (arg >> 16) & 0xff
    buf[3] = (arg >> 8) & 0xff
    buf[4] = arg & 0xff
    if (crc == 0):
        buf[5] = calculate_crc(buf[:-1])
    else:
        buf[5] = crc

    out = hex(buf[5])
    print("CMD: " + str(cmd) + " Arg: " + str(arg) + " CRC: " + hex(buf[5]))
    return 0

_cmd(0,0)
_cmd(8, 0x01aa)
_cmd(9, 0)
_cmd(41, 0x40000000)
_cmd(55, 0)
_cmd(58, 0)
_cmd(16, 512)
_cmd(12, 0)