#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Written By kenkeiras <kenkeiras@gmail.com>
# This program is free software. It comes without any warranty, to
# the extent permitted by applicable law. You can redistribute it
# and/or modify it under the terms of the Do What The Fuck You Want
# To Public License, Version 2, as published by Sam Hocevar. See
# http://sam.zoy.org/wtfpl/COPYING for more details.

import bluetooth, time, string
from select import select

# Rellena los datos
def padd(data, n, b = "\0"):
    l = len(data)
    while (l < n):
        data.append(b)
        l += 1
    return data

# Pasa un número de 3 bytes a str
def uint24str(n):
    data = []
    data.append( chr(n & 255) )
    n >>= 8

    data.append( chr(n & 255) )
    n >>= 8

    data.append( chr(n & 255) )

    data.reverse() # Little endian

    return ''.join(data)


# El controlador del mando
class pymote:

    writing_port = 0x11  # Puerto L2CAP de envío de información

    reading_port = 0x13  # Puerto L2CAP de recepción de información

    max_packet_size = 36 # Tamaño máximo de un paquete

    # Prepara el byte del led
    def get_led_byte(self):
        b = 0
        if (self.rumble):
            b |= 1

        if (self.led[ 0 ] != 0):
            b |= 0x10

        if (self.led[ 1 ] != 0):
            b |= 0x20

        if (self.led[ 2 ] != 0):
            b |= 0x40

        if (self.led[ 3 ] != 0):
            b |= 0x80
        return b

    # Ajusta los leds
    def set_leds(self):
        self.send(string.join([chr(0x52), chr(0x11),\
                                    chr(self.get_led_byte()) ], ""))


    # Activa/Desactiva los leds del mando
    def toogle_led(self, d = None):
        d = int(d)
        self.led[d - 1] ^= 1
        self.set_leds()
        pass

    # Pregunta el estado del mando
    def req_stat(self, d = None):
        b = 0
        if (self.rumble):
            b |= 1

        self.send(string.join([chr(0x52), chr(0x15), chr(b)], ""))

    # Activa/desactiva la vibración
    def toogle_rumble(self, d = None):
        if (self.rumble):
            self.rumble = False
        else:
            self.rumble = True
        self.req_stat(d)

    # Activa/desactiva el speaker
    def toogle_speaker(self, d = None):
        b = 0
        if (self.rumble):
            b |= 1

        if (not self.speaker):
            self.speaker = True
            b |= 4
            if (self.debug):
                print "Activado"
        else:
            self.speaker = False
            if (self.debug):
                print "Desactivado"

        self.send(string.join([chr(0x52), chr(0x14), chr(b)], ""))
        if (self.speaker):
            time.sleep(0.1)
            self.speaker_init()

    # Activa/desactiva la cámara IR
    # Esto es algo más complicado, aún sin implementar
    def toogle_ir(self, d = None):
        b = 0
        if (self.rumble):
            b |= 1

        if (not self.ir):
            self.ir = True
            b |= 4
            if (self.debug):
                print "Activado"
        else:
            self.ir = False
            if (self.debug):
                print "Desactivado"

        self.send(string.join([chr(0x52), chr(0x13), chr(b)], ""))
        time.sleep(0.1)
        self.send(string.join([chr(0x52), chr(0x1a), chr(b)], ""))
        time.sleep(0.1)

    # Escribe en un registro
    def reg_write(self, pos, data):
        b = 4
        if (self.rumble):
            b |= 1

        l = len(data)
        if (l > 16):
            raise Exception("Data too big =_=")
        data = ''.join(padd(data, 16))
        self.send(chr(0x52) + chr(0x16) + chr(b) + uint24str(pos) +
                   chr(l) + data)


    # Envia audio al mando
    def send_audio(self, data):
        l = len(data)
        if (l > 20):
            raise Exception("Data too big =_=")
        self.send(chr(0x52) + chr(0x18) + chr(l) + data)

    # Inicia el speaker
    # Con un bus de 4 Bits ADPCM y un framerate de 3000Hz
    def speaker_init(self):

        # Lo silencia
        self.mute = False
        self.toogle_mute()
        time.sleep(0.1)

        # Escribe 1 en el registro 0xa20009
        self.reg_write(0xa20009, [chr(0x01)])
        time.sleep(0.1)

        # Escribe 8 en el registro 0xa20001
        self.reg_write(0xa20001, [chr(0x08)])
        time.sleep(0.1)

        self.reg_write(0xa2001, [chr(0), chr(0x00), chr(0xD0), chr(0x07),\
                                 chr(0x40), chr(0), chr(0)])

        # Escribe 1 en el registro 0xa20008
        self.reg_write(0xa20008, [chr(0x01)])
        time.sleep(0.1)

        # Le devuelve el sonido
        self.toogle_mute()

    # Silencia/Da voz al speaker
    def toogle_mute(self, d = None):
        b = 0
        if (self.rumble):
            b |= 1

        if (not self.mute):
            self.mute = True
            b |= 4
            if (self.debug):
                print "Silenciado"
        else:
            self.mute = False
            if (self.debug):
                print "Con voz"

        self.send(string.join([chr(0x52), chr(0x19), chr(b)], ""))


    # Inicia el objeto y establece las conexiones
    def __init__(self, baddr, name = None, debug = False, client = True):

        self.debug = False
        self.led = [0, 1, 1, 0] # LED's
        self.rumble = False     # Vibración
        self.speaker = False    # Speaker
        self.mute = False       # Speaker silenciado
        self.ir = False         # Cámara IR
        self.baddr = baddr      # Dirección bluetooth
        self.name = None        # Nombre (no se usa)
        if (client):
            self.client_connect(baddr)
        else:
            self.server_connect(baddr)

        self.toogle_speaker()

    # Recibe el siguiente paquete de información
    def get_next(self):

        data = None
        (r, w, e) = select([self.r_s, self.w_s], [], [])
        if (self.r_s in r):
            data = self.r_s.recv(self.max_packet_size )
            if (len(data) < 1):
                self.connected = False

        elif (self.w_s in r):
            data = self.w_s.recv(self.max_packet_size )
            if (len(data) < 1):
                self.connected = False

        else:
            self.connected = False

        return data

    # Establece las conexiones directamente
    def client_connect(self, baddr):
        try:

            # Socket de escritura, envía información
            self.w_s = bluetooth.BluetoothSocket( bluetooth.L2CAP )
            self.w_s.connect((baddr, self.writing_port))

            # Socket de lectura, recibe información
            self.r_s = bluetooth.BluetoothSocket( bluetooth.L2CAP )
            self.r_s.connect((baddr, self.reading_port))

            self.connected = True

        except:
            raise Exception("Fallo al conectar")

    # Acepta conexiones nuevas
    def server_connect(self, baddr):
        try:

            # Socket de escritura, envía información
            self.w_s = bluetooth.BluetoothSocket( bluetooth.L2CAP )
            self.w_s.bind(("", self.writing_port))
            self.w_s.listen(1)
            (self.w_s, self.baddr) = self.w_s.accept()

            # Socket de escritura, envía información
            self.r_s = bluetooth.BluetoothSocket( bluetooth.L2CAP )
            self.r_s.bind(("", self.reading_port))
            self.r_s.listen(1)
            (self.r_s, self.baddr) = self.r_s.accept()

            self.connected = True

        except:
            raise Exception("¿Seguro que puedes abrir un puerto? ¬¬")

    # Envía algo
    def send(self, data):
        l = len(data)
        c = 0
        while (c < l):
            c += self.w_s.send(data[ c : ])

    # Cierra las conexiones
    def close(self):
        self.w_s.close()
        self.r_s.close()

    # Comprueba si está conectado
    def is_connected(self):
        return self.connected