"""
Some commonly used definitions for working with PyTekkotsu.
"""

dtor = lambda theta: (theta * 3.14159265358979) / 180.
clip = lambda x,a,b: [max(x, a), b][x > b] #clip x on (a..b)

#ports for the different Tekkotsu services
PORTS = {"Head Remote Control": 10052,
         "Walk Remote Control": 10050, 
         "EStop Remote Control": 10053,
         "World State Serializer": 10031,
         "Raw Cam Server": 10011,
         "Seg Cam Server": 10012,
         "Main Control": 10020,
         "Joint Writer": 10051,
         "Current PID Values": 10032}

#joint offsets
JOINTS = {"leg front left rotator": 0,
          "leg front left elevator": 1,
          "leg front left knee": 2,
          "leg front right rotator": 3,
          "leg front right elevator": 4,
          "leg front right knee": 5,
          "leg back left rotator": 6,
          "leg back left elevator": 7,
          "leg back left knee": 8,
          "leg back right rotator": 9,
          "leg back right elevator": 10,
          "leg back right knee": 11,
          "head tilt": 12,
          "head pan": 13,
          "head roll": 14,
          "head nod": 14,
          "tail tilt": 15,
          "tail pan": 16,
          "mouth": 17}

#button offsets
BUTTONS = {"paw front left": 0,
           "paw front right": 1,
           "paw back left": 2,
           "paw back right": 3,
           "chin": 4,
           "head": 5,
           "body front": 6,
           "body middle": 7,
           "body rear": 8,
           "wireless": 9}

#sensor offsets
SENSORS = {"ir near": 0,
           "ir far": 1,
           "ir chest": 2,
           "accel front-back": 3,
           "accel right-left": 4,
           "accel up-down": 5,
           "power remaining": 6,
           "power thermo": 7,
           "power capacity": 8,
           "power voltage": 9,
           "power current": 10}

#hardware limits (in degrees) for the various joints
RANGES = {"leg front left rotator": (-115,130),
          "leg front left elevator": (-10,88),
          "leg front left knee": (-25,122),
          "leg front right rotator": (-115,130),
          "leg front right elevator": (-10,88),
          "leg front right knee": (-25,122),
          "leg back left rotator": (-115,130),
          "leg back left elevator": (-10,88),
          "leg back left knee": (-25,122),
          "leg back right rotator": (-115,130),
          "leg back right elevator": (-10,88),
          "leg back right knee": (-25,122),
          "head tilt": (-75,0),
          "head pan": (-88,88),
          "head roll": (-15,45),
          "head nod": (-15,45),
          "tail tilt": (5,60),
          "tail pan": (-45,45),
          "mouth": (-55,-3)}

#convert to radians
RANGES = dict([(k, (dtor(v[0]), dtor(v[1]))) for (k, v) in RANGES.items()])

#to map .pos joint names -> our joint names
POSNAMES = {"NECK:tilt": "tilt",
          "NECK:pan~": "pan",
          "NECK:nod~": "roll",
          "NECK:roll": "roll",
          "MOUTH~~~~": "mouth",
          "LFr:rotor": "leg front left rotator",
          "LFr:elvtr": "leg front left elevator",
          "LFr:knee~": "leg front left knee",
          "LBk:rotor": "leg back left rotator",
          "LBk:elvtr": "leg back left elevator",
          "LBk:knee~": "leg back left knee",
          "RFr:rotor": "leg front right rotator",
          "RFr:elvtr": "leg front right elevator",
          "RFr:knee~": "leg front right knee",
          "RBk:rotor": "leg back right rotator",
          "RBk:elvtr": "leg back right elevator",
          "RBk:knee~": "leg back right knee",
          "TAIL:tilt": "tail tilt",
          "TAIL:pan~": "tail pan"}