import os

def set_servos(a, b):
  # The /dev entry we need to use annoyingly keeps changing.
  cmd = "./arduino-serial -b 19200 -p /dev/tty.usbserial-A50018fz15 -s a%02d%02d" % (a, b)
  os.system(cmd)
  pass

pipe = None

def read_sms(num_samples):
  global pipe
  if not pipe:
    cmd = "./smsutil -i 0.015 -c 0"
    pipein, pipeout = os.popen2(cmd, "r", 0)
    pipe = pipeout
  (x, y, z) = (0.0, 0.0, 0.0)
  for i in range(0, num_samples):
    line = ''
    while line == '':
      line = pipe.readline()
    (xstr, ystr, zstr) = line.split()
    x += float(xstr)
    y += float(ystr)
    z += float(zstr)
  return (x / num_samples, y / num_samples, z / num_samples)

def cook(n):
  n = int((n + 1) * 50.0)
  if n < 0:
    n = 0
  if n > 99:
    n = 99
  return n

def run():
  while True:
    (x, y, z) = read_sms(3)
    x = cook(x)
    y = cook(-y)
    set_servos(x, y)


if __name__ == '__main__':
  run()