'''Wiiboard driver
Nedim Jackman December 2008
No liability held for any use of this software.
More information at http://code.google.com/p/wiiboard-simple/
'''

import bluetooth
import sys
import _thread
import time
import binascii

CONTINUOUS_REPORTING = 0x04

COMMAND_LIGHT = 0x11
COMMAND_REPORTING = 0x12
COMMAND_REQUEST_STATUS = 0x15
COMMAND_REGISTER = 0x16
COMMAND_READ_REGISTER = 0x17

#input is Wii device to host
INPUT_STATUS = 0x20
INPUT_READ_DATA = 0x21

EXTENSION_8BYTES = 0x32

BUTTON_DOWN_MASK = 8

BATTERY_MAX = 0xD0 # Taken from cwiid source

TOP_RIGHT = 0
BOTTOM_RIGHT = 1
TOP_LEFT = 2
BOTTOM_LEFT = 3

BLUETOOTH_NAME = "Nintendo RVL-WBC-01"


class BoardEvent:
    def __init__(self, topLeft,topRight,bottomLeft,bottomRight, buttonPressed, buttonReleased):

        self.topLeft = topLeft
        self.topRight = topRight
        self.bottomLeft = bottomLeft
        self.bottomRight = bottomRight
        self.buttonPressed = buttonPressed
        self.buttonReleased = buttonReleased
        #convenience value
        self.totalWeight = topLeft + topRight + bottomLeft + bottomRight

class Wiiboard:
    def __init__(self):
        self.datasocket = None
        self.controlsocket = None
        self.calibration = []
        self.calibrationRequested = False
        self.LED = False
        self.address = None
        self.buttonDown = False
        self.reportingTypeSet = False
        self.battery = None
        for i in range(3):
            self.calibration.append([])
            for j in range(4):
                self.calibration[i].append(10000) #high dummy value so events with it don't register

        self.status = "Disconnected"
        self.lastEvent = BoardEvent(0,0,0,0,False,False)
        self.mass = self.lastEvent

        try:
            self.datasocket = bluetooth.BluetoothSocket(bluetooth.L2CAP)
            self.controlsocket = bluetooth.BluetoothSocket(bluetooth.L2CAP)
        except ValueError:
            raise Exception("Error: Bluetooth not found")

    def isConnected(self):
        if self.status == "Connected":
            return True
        else:
            return False

    # Connect to the Wiiboard at bluetooth address <address>
    def connect(self, address):
        if address == None:
            print("Non existant address")
            return
        self.datasocket.connect((address, 0x13))
        self.datasocket.settimeout(2)
        self.controlsocket.connect((address, 0x11))
        self.controlsocket.settimeout(2)
        if self.datasocket and self.controlsocket:
            print("Connected to Wiiboard at address " + address)
            self.status = "Connected"
            self.address = address
            _thread.start_new_thread(self.receivethread, ())
            _thread.start_new_thread(self.statusthread, ())
            self.calibrate()
            useExt = bytes([COMMAND_REGISTER, 0x04, 0xA4, 0x00, 0x40, 0x00])
            self.send(useExt)
            self.setReportingType()
        else:
            print("Could not connect to Wiiboard at address " + address)

    # Disconnect from the Wiiboard
    def disconnect(self):
        if self.status == "Connected":
            self.status = "Disconnecting"
            while self.status == "Disconnecting":
                time.sleep(0.001)
        try:
            self.datasocket.close()
            self.controlsocket.close()
        except:
            pass
        print("WiiBoard disconnected")

    # Try to discover a Wiiboard
    def discover(self):
        #print "Press the red sync button on the board now"
        address = None
        bluetoothdevices = bluetooth.discover_devices(duration = 6, lookup_names = True)
        for addr, name in bluetoothdevices:
            if name == BLUETOOTH_NAME:
                address = addr
                print("Found Wiiboard at address " + address)
        #if address == None:
            #print "No Wiiboards discovered."
        return address

    def createBoardEvent(self, data):
        buttonBytes = data[0:2]
        data = data[2:12]
        buttonPressed = False
        buttonReleased = False

        state = (buttonBytes[0] << 8) | buttonBytes[1]
        if state == BUTTON_DOWN_MASK:
            buttonPressed = True
            if not self.buttonDown:
                self.buttonDown = True

        if buttonPressed == False:
            if self.lastEvent.buttonPressed == True:
                buttonReleased = True
                self.buttonDown = False

        rawTR = (data[0] << 8) + data[1]
        rawBR = (data[2] << 8) + data[3]
        rawTL = (data[4] << 8) + data[5]
        rawBL = (data[6] << 8) + data[7]

        topLeft = self.calcMass(rawTL, TOP_LEFT)
        topRight = self.calcMass(rawTR, TOP_RIGHT)
        bottomLeft = self.calcMass(rawBL, BOTTOM_LEFT)
        bottomRight = self.calcMass(rawBR, BOTTOM_RIGHT)
        boardEvent = BoardEvent(topLeft,topRight,bottomLeft,bottomRight,buttonPressed,buttonReleased)
        return boardEvent


    def calcMass(self, raw, pos):
        val = 0.0
        #calibration[0] is calibration values for 0kg
        #calibration[1] is calibration values for 17kg
        #calibration[2] is calibration values for 34kg
        if raw < self.calibration[0][pos]:
            return val
        elif raw < self.calibration[1][pos]:
            val = 17 * ((raw - self.calibration[0][pos]) / float((self.calibration[1][pos] - self.calibration[0][pos])))
        elif raw > self.calibration[1][pos]:
            val = 17 + 17 * ((raw - self.calibration[1][pos]) / float((self.calibration[2][pos] - self.calibration[1][pos])))

        return val

    def getEvent(self):
        return self.lastEvent
        
    def getLED(self):
        return self.LED

    # Thread that listens for incoming data
    def receivethread(self):
        while self.status == "Connected":
            try:
                data = self.datasocket.recv(25)
                intype = data[1]
                if intype == INPUT_STATUS:
                    self.battery = data[7]/BATTERY_MAX
                    if not self.reportingTypeSet:
                        self.setReportingType()
                elif intype == INPUT_READ_DATA:
                    if self.calibrationRequested == True:
                        packetLength = data[4]//16 + 1
                        self.parseCalibrationResponse(data[7:(7+packetLength)])

                        if packetLength < 16:
                            self.calibrationRequested = False

                elif intype == EXTENSION_8BYTES:
                    self.lastEvent = self.createBoardEvent(data[2:12])
                    self.mass = self.lastEvent

                else:
                    print("ACK to data write received")
            except bluetooth.btcommon.BluetoothError:
                print('Error! Disconnecting')
                self.disconnect()

        self.status = "Disconnected"
        self.disconnect()

    def statusthread(self):
        while self.status == "Connected":
            time.sleep(30)
            self.requestStatus()

    def parseCalibrationResponse(self, bytes):
        index = 0
        if len(bytes) == 16:
            for i in range(2):
                for j in range(4):
                    self.calibration[i][j] = (bytes[index] << 8) + bytes[index+1]
                    index += 2
        elif len(bytes) < 16:
            for i in range(4):
                self.calibration[2][i] = (bytes[index] << 8) + bytes[index+1]
                index += 2

    # Send <data> to the Wiiboard
    # <data> should be of type bytes
    def send(self, data):
        if self.status != "Connected":
            return
        senddata = b'\xa2'+data

        self.datasocket.send(senddata)
        
    def setLight(self, light):
        val = 0x00
        if light == True:
            val = 0x10

        msg = bytes([COMMAND_LIGHT, val])
        self.send(msg)
        self.LED = light

    def calibrate(self):
        msg = bytes([COMMAND_READ_REGISTER, 0x04, 0xA4, 0x00, 0x24, 0x00, 0x18])
        self.send(msg)
        self.calibrationRequested = True

    def setReportingType(self):
        msg = bytes([COMMAND_REPORTING, CONTINUOUS_REPORTING, EXTENSION_8BYTES])
        self.send(msg)
        self.reportingTypeSet = True

    def requestStatus(self):
        msg = bytes([COMMAND_REQUEST_STATUS, 0x00])
        self.send(msg)