import sys
from PyQt4 import QtCore, QtGui
from PyQt4.Qt import QThread

from wimot import *
import random
from threading import Thread
import cwiid
import time

import os
import os.path
import signal

os.chdir('/tmp')

'''['__class__', '__delattr__', '__doc__', '__getattribute__', '__hash__', '__init__', '__new__', 
'__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__str__', 'close', 'disable', 'enable', 
'get_acc_cal', 'get_mesg', 'led', 'mesg_callback', 'read', 'request_status', 'rpt_mode', 'rumble', 
'state', 'write']

['led', 'rpt_mode', 'ext_type', 'rumble', 'error', 'battery']
'''

def fifo_write(fd, msg):
    fd.write('%04i' % len(msg))
    fd.write(msg)
    fd.flush()

def fifo_read(fd):
    x = fd.read(4)
    if not x:
        raise IOError('majek')
    a = int(x)
    return fd.read(a)


WIIMOTES=[
    '00:19:1D:C3:D2:B4', # nowy
    '00:19:1D:AA:F9:20', # stary
    ]

counter = 0

class WiimoteThread(Thread):
    wm_id   = None
    bdaddr  = None
    def __init__ (self, wm_id, bdaddr):
        Thread.__init__(self)
        self.wm_id  = wm_id
        self.bdaddr = bdaddr
    
    def run(self):
        global counter
        fifoq = 'wii-%s-query' % self.bdaddr.replace(':','')
        fifor = 'wii-%s-resp' % self.bdaddr.replace(':','')
        if not os.path.exists(fifoq):
            os.mkfifo(fifoq)
        if not os.path.exists(fifor):
            os.mkfifo(fifor)
        
        print "%r Waiting. Press 1+2 buttons." % self.bdaddr
        wii = cwiid.Wiimote(self.bdaddr)
        print "%r Connected" % self.bdaddr
        enabled = False
        

        while True:
            try:
                fdq = open(fifoq, 'r')
                fdr = open(fifor, 'w')
                print "Connected to soft..."
                last_none = False
                start = time.time()
                zeroes = 0
                while True:
                    a = fifo_read(fdq)
                    query = eval( a )
                    
                    if isinstance(query, tuple):
                        parameter = query[1]
                        query     = query[0]
                        fifo_write(fdr, '%r' % parameter)
                    if query == 'led':
                        wii.led = parameter
                    elif query == 'battery':
                        wii.request_status()
                        bat =  (float(wii.state['battery']) / 256.0 ) * 100.0
                        fifo_write(fdr, '%r' % bat)
                    elif query == 'irda':
                        if not enabled:
                            print "enabled"
                            #wii.mesg_callback = cb
                            t0 = time.time()
                            wii.rpt_mode = cwiid.CMD_RPT_MODE | cwiid.RPT_IR
                            wii.enable(cwiid.FLAG_MESG_IFC| cwiid.FLAG_CONTINUOUS| cwiid.FLAG_NONBLOCK)
                            while  wii.get_mesg() == 0: pass
                            wii.disable(cwiid.FLAG_MESG_IFC| cwiid.FLAG_CONTINUOUS| cwiid.FLAG_NONBLOCK)
                            wii.enable(cwiid.FLAG_MESG_IFC| cwiid.FLAG_CONTINUOUS)
                            t1 = time.time()
                            print "setting %f" % ((t1-t0)*1000.0)
                            enabled = True
                        
                        while True:
                            data_list = wii.get_mesg()
                            t1 = time.time()
                            if (t1 - start)*1000.0 < 8:
                                continue
                            else:
                                start = t1
                                break
                        
                        
                        datas = []
                        for typ, data in data_list:
                            if typ == 3 and len(data) == 4:
                                data = filter(lambda a:a, data)
                                data = map(lambda d: (float(1024.0-d['pos'][0])/1024.0, float(d['pos'][1])/768.0, d['size']), data)
                                datas.extend(data)
                        fifo_write(fdr, '%r' % datas)
                    else:
                        print "error"
            except IOError, NameError:
                pass
            try:
                fdr.close()
            except IOError:
                pass
            try:
                fdq.close()
            except IOError:
                pass
            if enabled:
                print "disabled"
                enabled=False
                wii.disable(cwiid.FLAG_MESG_IFC| cwiid.FLAG_CONTINUOUS)
                wii.rpt_mode = 0
            wii.led = 0x0
            print "Disconnected"
            time.sleep(1)
            
        wii.close()


if __name__ == '__main__':
    for i, bdaddr in enumerate(WIIMOTES):
        t = WiimoteThread(i, bdaddr)
        t.start()
    
    #t.join()