import struct


def data2stack(data):
    """
    Convert binary ocotomap data into three lists (occupied, free, unknown)
    :param data: binary octomap data (depth first)
    :return: (occupied, free, unknown) lists of sequences
    """
    stack = [[]]
    unknown = []
    free = []
    occupied = []
    for i in range(len(data) // 2):
        prefix = stack.pop(0)
        d = struct.unpack_from('<H', data, i * 2)[0]
        for rot in range(14, -2, -2):  # range(0, 16, 2):
            val = (d & (0x3 << rot)) >> rot
            if val == 3:
                stack.insert(0, prefix + [rot // 2])
            elif val == 2:
                occupied.append(prefix + [rot // 2])
            elif val == 1:
                free.append(prefix + [rot // 2])
            elif val == 0:
                unknown.append(prefix + [rot // 2])
    assert len(stack) == 0, len(stack)
    return occupied, free, unknown