"""
Slightly better implementation of using the joystick
Does not use the pytekkotsu AiboRobot class
"""

from pygame import *
from time import sleep
from listener import Listener
from defs import PORTS, clip
import struct

### variables and setup #######################################################

ip = "aibo4" #192.168.0.44

PS2BUTTONS= {0: "TRIANGLE",
             1: "CIRCLE",
             2: "CROSS",
             3: "SQUARE",
             4: "L1",
             5: "R1",
             6: "L2",
             7: "R2",
             8: "START",
             9: "SELECT",
             10: "L3",
             11: "R3"}

x,y,z = 0,0,0
p,t,r = 0,0,0

main_control = Listener(PORTS["Main Control"], ip)
main_control.s.send("!reset\n") # reset menu
main_control.s.send("!select \"%s\"\n" % "Walk Remote Control")
#main_control.s.send("!select \"%s\"\n" % "UPenn Walk Remote Control") # woah!
main_control.s.send("!select \"%s\"\n" % "Head Remote Control")
walk_control = Listener(PORTS["Walk Remote Control"], ip)
head_control = Listener(PORTS["Head Remote Control"], ip)


### functions #################################################################

def timestep(sec):
    while True:
        print "sleep"
        yield True
        sleep(sec)

def makecc(control, amt):
    return struct.pack('<bf', ord(control), amt) 

def setTargetVelocity():
    global x,y,z
    walk_control.write(makecc('f', x))
    walk_control.write(makecc('s', y))
    walk_control.write(makecc('r', z))

def pan_tilt_roll():
    global p,t,r
    p = clip(p, -1.0, 1.0)
    t = clip(t, -1.0, 1.0)
    r = clip(r, -1.0, 1.0)
    head_control.write(makecc('t', t))
    head_control.write(makecc('p', p))
    head_control.write(makecc('r', r))

def process(eventdict, eventtype):
    global x,y,z,p,t,r
    if "joy" in eventdict:
        if "value" in eventdict:
            if "axis" in eventdict:
                if eventdict["axis"] == 0:
                    z = -1*eventdict["value"]
                elif eventdict["axis"] == 1:
                    x = -1*eventdict["value"]
                elif eventdict["axis"] == 3:
                    y = -1*eventdict["value"]
            elif "hat" in eventdict:
              print eventdict["hat"], "hat, value = ", eventdict["value"]
        elif "button" in eventdict:
            if eventtype == JOYBUTTONDOWN:
                b = eventdict["button"]
                if PS2BUTTONS[b] == "SQUARE":
                    p += 0.2
                elif PS2BUTTONS[b] == "CIRCLE":
                    p += -0.2
                elif PS2BUTTONS[b] == "TRIANGLE":
                    t += 0.2
                elif PS2BUTTONS[b] == "CROSS":
                    t += -0.2
                elif PS2BUTTONS[b] == "L1":
                    r += -0.2
                elif PS2BUTTONS[b] == "L2":
                    r += 0.2
                print PS2BUTTONS[b], "pressed"

### init ######################################################################

init()
joystick.init()
for x in range(joystick.get_count()):
    j = joystick.Joystick(x)
    j.init()

pan_tilt_roll()

### main ######################################################################

count = 0
while True:
    count += 1
    if count % 10000 == 0:
        print "sending (%.8f, %.8f, %.8f), (%.8f, %.8f, %.8f)" % (x,y,z,p,t,r)
        setTargetVelocity()
        pan_tilt_roll()
        for e in event.get():
            process(e.dict, e.type)