#!/usr/bin/env python3

# This class is used for interfacing with the DWM1001 Decawave modules

import time
import spidev

class Decawave:
    def __init__(self, spibus):
        self.spibus = spibus
        self.tlv_type = {
                        'POS_GET': 0x02
                        }

    def response_ready(self, expect):
        size_num = [0, 0]
        while size_num == [0, 0]:
            size_num = self.spibus.readbytes(2)
            time.sleep(0.001)

        if size_num != expect:
            return False

        return True

    return True

    def send(request):
        send_bytes = [request.type, request.length] 
        if request.length != 0:
            send_bytes = send_bytes + request.value

        self.spibus.writebytes(send_bytes)
        return

    def receive(response, expect):
        while not self.response_ready(expect):
            pass
        
        response_bytes = self.spibus.readbytes(expect[0])
        response.type = response_bytes[0]
        response.length = response_bytes[1]
        response.value = response_bytes[2:]
        return

    def pos_get(self):
        length = 0x00
        expect = [18, 1]
        request = tlv.tlv(self.tlv_type['POS_GET'], length)

        self.send(request)
        response = tlv.tlv()
        self.receive(response, expect)
        value = response.value

        x = response.value[0] + (response.value[1] << 8) + (response.value[2] << 16) + (response.value[3] << 24)
        y = response.value[4] + (response.value[5] << 8) + (response.value[6] << 16) + (response.value[7] << 24)
        z = response.value[8] + (response.value[9] << 8) + (response.value[10] << 16) + (response.value[11] << 24)
        quality = response.value[12]
        
        return [x, y, z, quality]

class tlv:
    def __init__(self, type=0, length=0, value=[]):
        self.type = type
        self.length = length
        self.value = value