import serial
import pygame
import struct
import os
import sys
import time
class TextPrint:
    def __init__(self):
        self.reset()
        self.font = pygame.font.Font(None, 30)

    def show(self, screen, textString):
        textBitmap = self.font.render(textString, True, (0,0,0))
        screen.blit(textBitmap, [self.x, self.y])
        self.y += self.line_height
        
    def reset(self):
        self.x = 20
        self.y = 20
        self.line_height = 25
        
    def indent(self):
        self.x += 20
        
    def unindent(self):
        self.x -= 20

class AxisPipe:
    def __init__(self, port):
        self.port   =   port
        self.serial =   serial.Serial(port,38400, timeout=0.01)
        self.axis   =   [ 0,0,0,0 ]
        self.aux    =   [ 0,0 ]
        
    def UpdateAxis(self, axis, value):
        if self.axis[axis] != value & 0xFFFF:
            print "Updating Axis %s with %s" %(axis,value)
            self.axis[axis] = value & 0xFFFF
            self.serial.write(struct.pack("BBB",axis,value&0xFF,value/0xFF))
            
    def UpdateAux(self, aux, value):
        if self.aux[aux] != value & 0xFFFF:
            print "Updating Aux %s with %s" %(aux,value)
            self.aux[aux] = value & 0xFFFF
            self.serial.write(struct.pack("BBB",aux+7,value&0xFF,value/0xFF))
            
    def SetLed(self, val):
        self.serial.write(struct.pack("BBB",4,val&0xFF,0))
        
    def ReadData(self):
        data = self.serial.readline()
        if len(data) > 1:
            print "Response: %s" %data.replace("\n","")
            
    def TurnOn(self):
        self.serial.write("\x05\x00\x00")
        self.UpdateAxis(0,1000);
        self.UpdateAxis(1,1000);
        self.UpdateAxis(2,1000);
        self.UpdateAxis(3,1000);

        self.UpdateAux(0,1000);
        self.UpdateAux(1,1000);

    def TurnOff(self):
        self.serial.write("\x06\x00\x00")
        
    def KeepAlive(self):
        self.serial.write("\x09\x00\x00")

    def Close(self):
        self.TurnOff()
        self.serial.close()     
        
pygame.init()

screen = pygame.display.set_mode([540,300])
pygame.display.set_caption("Controller")

done = False
ax = AxisPipe("/dev/rfcomm21")
led = 0
leddir = False
clock = pygame.time.Clock()


pygame.joystick.init()
textPrint = TextPrint()

def FixDeadBand(val):
    if val >= 1580 or val <= 1420:
        return val
    else:
        return 1500

ax.UpdateAxis(0,1000);
ax.UpdateAxis(1,1000);
ax.UpdateAxis(2,1000);
ax.UpdateAxis(3,1000);

ax.UpdateAux(0,1000);
ax.UpdateAux(1,1000);

while done==False:
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            done=True
        elif event.type == pygame.JOYBUTTONUP:
            if event.button == 7:
                print "Turning ON the controller"
                ax.TurnOn()
            elif event.button == 6:
                print "Turning OFF the controller"
                ax.TurnOff()
            elif event.button == 0: #   Button A
                ax.UpdateAux(0,int(joystick.get_axis(5)*500+1500))
            elif event.button == 1: #   Button B
                ax.UpdateAux(1,int(joystick.get_axis(5)*500+1500))
        elif event.type == pygame.KEYUP:
            if event.key == 100:
                ax.UpdateAxis(1,2000)
            elif event.key == 97:
                ax.UpdateAxis(1,1000)
            else:
                ax.UpdateAxis(1,1500)
    
    screen.fill((255,255,255))
    
    textPrint.reset()
                
    if  pygame.joystick.get_count() > 0:
        joystick = pygame.joystick.Joystick(1)
        joystick.init()
    
        textPrint.show(screen, "Joystick {}".format(0) )
        textPrint.indent()     
        name = joystick.get_name()
        textPrint.show(screen, "Joystick name: {}".format(name) )
        if joystick.get_numaxes() >= 6:
            ax.UpdateAxis(0,int(joystick.get_axis(4)*500+1500))
            ax.UpdateAxis(1,FixDeadBand(int(joystick.get_axis(2)*250+1500)))
            ax.UpdateAxis(2,FixDeadBand(int(-joystick.get_axis(3)*250+1500)))
            ax.UpdateAxis(3,FixDeadBand(int(joystick.get_axis(0)*500+1500)))
            textPrint.indent()
            textPrint.show(screen, "CH1:  {}".format(ax.axis[0]))
            textPrint.show(screen, "CH2:  {}".format(ax.axis[1]))
            textPrint.show(screen, "CH3:  {}".format(ax.axis[2]))
            textPrint.show(screen, "CH4:  {}".format(ax.axis[3]))
            textPrint.show(screen, "AUX0: {}".format(ax.aux[0]))
            textPrint.show(screen, "AUX1: {}".format(ax.aux[1]))
            textPrint.unindent()

    else:
        textPrint.show(screen, "No Joystick Available" )
        ax.UpdateAxis(0,0);
        ax.UpdateAxis(1,0);
        ax.UpdateAxis(2,0);
        ax.UpdateAxis(3,0);
        
    ax.ReadData() 
    if leddir:
        if led >= 255:
            led = 255
            leddir = False
        else:
            led += 5
    else:
        if led <= 0:
            led = 0
            leddir = True
        else:
            led -= 5  
    #ax.SetLed(led);
    textPrint.show(screen, "LED: {}".format(led) )
    textPrint.unindent()
    textPrint.unindent()             
    pygame.display.flip()   
    ax.KeepAlive()
    clock.tick(240) #   We need to update the KeepAlive more than 70 times per sec
    
ax.Close()
pygame.quit ()