blup/wii-pair/daemon.py

#!/usr/bin/env python3
import sys
import os, math, random
import time
import socket
import struct
import select
from threading import Thread
import bluetooth
import wiiboard

known_boards = {'niklas': '00:26:59:34:C8:69',
'kaka': '00:26:59:37:04:15',
'michael': '00:24:44:65:5B:F8',
'fed': '00:23:CC:23:5E:1D',
}

MIN_WEIGHT = 10
PLAYER_TIMEOUT = 3

class StatusThread(Thread):
    def __init__(self, t1, t2):
        Thread.__init__(self)
        self.t1 = t1
        self.t2 = t2
        self.stop = False

    def run(self):
        t1 = self.t1
        t2 = self.t2
        colw = 50
        batw = colw - len('Bat: 000% ||  ')
        while not self.stop:
            print('')
            print(('Left player - '+t1.addr).ljust(colw) + 'Right player - '+t2.addr)
            print('-'*2*colw)
            if t1.addr != 'dummy':
                if t1.connected:
                    s1 = 'Connected'
                    bat = t1.board.battery
                else:
                    s1 = 'Connecting...'
                    bat = -1
            else:
                s1 = 'Dummy'
                bat = 0.42
            px = 'Low  ' if t1.pos_x is None else '%.3f'%t1.pos_x
            py = 'Low  ' if t1.pos_y is None else '%.3f'%t1.pos_y
            p1 = 'Position: X: %s - Y: %s' % (px, py)
            if bat is not None:
                b1 = 'Bat: |%s| %03d%%' % (('='*int(batw*bat)).ljust(batw), int(bat*100))
            else:
                b1 = 'Bat: |%s| ---%%' % ('?'*batw).ljust(batw)
            w1 = 'Weight: %.1f' % t1.weight

            if t2.addr != 'dummy':
                if t2.connected:
                    s2 = 'Connected'
                    bat = t2.board.battery
                else:
                    s2 = 'Connecting...'
                    bat = -1
            else:
                s2 = 'Dummy'
                bat = 0.42
            px = 'Low  ' if t2.pos_x is None else '%.3f'%t2.pos_x
            py = 'Low  ' if t2.pos_y is None else '%.3f'%t2.pos_y
            p2 = 'Position: X: %s - Y: %s' % (px, py)
            if bat is not None:
                b2 = 'Bat: |%s| %03d%%' % (('='*int(batw*bat)).ljust(batw), int(bat*100))
            else:
                b2 = 'Bat: |%s| ---%%' % ('?'*batw).ljust(batw)
            w2 = 'Weight: %.1f' % t2.weight

            print(s1.ljust(colw) + s2)
            print(b1.ljust(colw) + b2)
            print(w1.ljust(colw) + w2)
            print(p1.ljust(colw) + p2)
            sys.stdout.flush()

            time.sleep(1)

class WiiThread(Thread):
    def __init__(self, player, addr):
        Thread.__init__(self)
        self.addr = addr
        self.player = player
        self.stop = False
        self.connected = False
        self.board = None
        self.pos_x = None
        self.pos_y = None
        self.weight = 0
        self.timeout = None

    def run(self):
        if self.addr != 'dummy':
            while not self.stop:
                self.connected = False
                self.pos = 'C'
                self.board = wiiboard.Wiiboard()
                print('%s: Connecting to %s' % (self.player, self.addr))
                try:
                    self.board.connect(self.addr)
                except bluetooth.btcommon.BluetoothError:
                    continue

                time.sleep(1)
                print('%s: Connected' % self.player)
                self.connected = True
                self.board.setLight(True)

                while not self.stop and self.board.status == 'Connected':
                    self.weight = self.board.mass.totalWeight
                    if self.board.mass.totalWeight >= MIN_WEIGHT:
                        self.timeout = None
                        m = self.board.mass
                        x_balance = -(m.topLeft+m.bottomLeft) + (m.topRight+m.bottomRight)
                        x_balance = x_balance/float(m.totalWeight)
                        y_balance = -(m.bottomLeft+m.bottomRight) + (m.topLeft+m.topRight)
                        y_balance = y_balance/float(m.totalWeight)
                        self.pos_x = x_balance
                        self.pos_y = y_balance
                    else:
                        if self.timeout is None:
                            self.timeout = time.time()+PLAYER_TIMEOUT
                        else:
                            if time.time() >= self.timeout:
                                self.pos_x = None
                                self.pos_y = None

                    time.sleep(0.1)

            self.board.disconnect()
        else:
            print('%s: Connected as DUMMY' % self.player)
            self.connected = True
            self.pos = 'D'

wiis = []
if len(sys.argv) > 1:
    if sys.argv[1] in list(known_boards.keys()):
        addr = known_boards[sys.argv[1]]
    else:
        addr = sys.argv[1]
    t1 = WiiThread('L', addr)
else:
    t1 = WiiThread('')

t1.daemon = True
t1.start()

if len(sys.argv) > 2:
    if sys.argv[2] in list(known_boards.keys()):
        addr = known_boards[sys.argv[2]]
    else:
        addr = sys.argv[2]
    t2 = WiiThread('R', addr)
else:
    t2 = WiiThread('')

while not t1.connected:
    time.sleep(0.1)

t2.daemon = True
t2.start()

while not t2.connected:
    time.sleep(0.1)

tStatus = None
if os.environ.get('DEBUG_STATUS', 0) == '1':
    tStatus = StatusThread(t1, t2)
    tStatus.daemon = True
    tStatus.start()

wiis = [t1, t2]

s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
s.bind(('', 4711))
s.listen(1)

try:
    conns = []
    while True:
        rlist, wlist, xlist = select.select([s]+conns, [], conns)
        if s in rlist:
            conn, addr = s.accept()
            conns.append(conn)
            rlist.remove(s)

        for sock in xlist:
            conns.remove(sock)

        for sock in rlist:
            try:
                m = sock.recv(1)
            except Exception:
                conns.remove(sock)
                continue

            if len(m) == 0:
                conns.remove(sock)
                continue

            r = b''
            for w in wiis:
                if m[0] == ord('w'):
                    # Return weight
                    v = 0
                    if w.weight is not None:
                        v = round(w.weight)
                    r += struct.pack('B', v)
                elif m[0] == ord('b'):
                    # Return battery
                    v = 0
                    if w.board.battery is not None:
                        v = round(w.board.battery*100)
                    r += struct.pack('B', v)
                else:
                    # Return balance
                    if w.pos_x is None:
                        v = -128
                    else:
                        v = int(round(w.pos_x*100))
                    r += struct.pack('b', v)
                    if w.pos_y is None:
                        v = -128
                    else:
                        v = int(round(w.pos_y*100))
                    r += struct.pack('b', v)

            try:
                sock.send(r)
            except Exception:
                conns.remove(sock)
                continue

except (KeyboardInterrupt, SystemExit):
    t1.stop = True
    t2.stop = True
    if tStatus is not None:
        tStatus.stop = True
    sys.exit()