--- a/leg.py	Tue Jan 20 01:49:59 2015 +0100
+++ b/leg.py	Tue Jan 20 01:50:14 2015 +0100
@@ -1,22 +1,73 @@
 import math
 
 
-class Leg(object):
-    body = 36.0
+US_MIN = 600
+US_MAX = 2400
+US_CENTER = US_MIN + (US_MAX - US_MIN) // 2
+US_PER_RADIAN = float(US_MAX - US_MIN) / math.pi
+
+
+def _solve_triangle(a, b, c):
+    """Calculate an angle of a triangle using the law of cosines."""
+    a, b, c = abs(a), abs(b), abs(c)
+    if a + b < c or a + c < b or b + c < a:
+        raise ValueError("Impossible triangle: %r, %r, %r" % (a, b, c))
+    cos = float(a ** 2 + b ** 2 - c ** 2) / (2 * a * b)
+    return math.acos(cos)
+
+
+def _norm(a, b):
+    """Calculate the length of a vector."""
+    return math.sqrt(a ** 2 + b ** 2)
+
+
+class Leg:
+    body_radius = 36.0
     coxa = 33.0
     femur = 43.0
     tibia = 53.0
 
-    def __init__(self, servos, angle):
-        angle = math.radians(angle)
-        self.servos = servos
+    def __init__(self, servos, angle=0, trims=(0, 0, 0)):
         self.angle = angle
+        self.servos = servos
+        self.trims = trims
         self.base = (
-            self.body * math.cos(angle),
-            self.body * math.sin(angle)
+            self.body_radius * math.cos(self.angle),
+            self.body_radius * math.sin(self.angle)
         )
         self.home = (
-            (self.coxa + self.femur) * math.cos(angle),
-            (self.coxa + self.femur) * math.sin(angle),
+            (self.coxa + self.femur) * math.cos(self.angle),
+            (self.coxa + self.femur) * math.sin(self.angle),
             -self.tibia,
         )
+        self.position = [0, 0, 0]
+        self.move_to(*self.home)
+
+    def _inverse_kinematics(self, x, y, z):
+        """Calculate the 3 angles of the leg, in radians."""
+        f = _norm(x, y) - self.coxa
+        d = min(self.femur + self.tibia, _norm(f, z))
+        hip = math.atan2(y, x)
+        knee = _solve_triangle(self.femur, d, self.tibia) - math.atan2(-z, f)
+        ankle = _solve_triangle(self.femur, self.tibia, d)
+        return ankle, knee, hip
+
+    def move_to(self, x=None, y=None, z=None):
+        x, y, z = [given if given is not None else current
+                   for (given, current) in zip((x, y, z), self.position)]
+        angles = list(self._inverse_kinematics(x, y, z))
+        angles[0] += self.angle
+        self.angles = angles
+        self.position = x, y ,z
+
+    def move_by(self, dx, dy, dz):
+        x, y, z = self.position
+        self.move_to(x + dx, y + dy, z + dz)
+
+    def update(self, controller):
+        for servo, angle, trim in zip(self.servos, self.angles, self.trims):
+            while angle > math.pi:
+                angle -= math.pi * 2
+            us = US_CENTER + angle * US_PER_RADIAN + trim
+            target = min(US_MAX * 4, max(US_MIN * 4, int(us * 4)))
+            controller.set_target(servo, target)