#! /usr/bin/env python

import sys,string
from socket import *
from thread import start_new_thread

ECHO_PORT = 50007
BUFSIZE = 1024

debug = 0



def ClientRecvThread(_sock,_location):
    print 'Thread Function'
    while 1:
      try:
        recv=_sock.recv(1000)
        ParseData(recv)
        SendUpdate(_sock)
      except:
        pass

def main():
    if len(sys.argv) > 1:
        port = int(eval(sys.argv[1]))
    else:
        port = ECHO_PORT
    s = socket(AF_INET, SOCK_STREAM)
    s.bind(('', port))
    s.listen(1)
    while 1==1:
         NewSocket,location = s.accept()
         print 'connected by', location
         start_new_thread(ClientRecvThread,(NewSocket,location))


def ParseData(_data):
    cmdBegin = 0
    cmdEnd = 1024
    cmdBegin = _data.find("!!",cmdBegin,cmdEnd)
    while (cmdBegin>=0):
      start = _data.find('=',cmdBegin,cmdEnd)
      end   = _data.find(',',cmdBegin,cmdEnd)

      command = _data[start+1:end]
      if (debug > 0):
        print command
      if (command == 'init'):
        ProcInit (_data)
      elif (command == 'end'):  
        ProcEnd(_data)
      elif (command == 'speedupdate'):  
        ProcSpeedUpdate(_data[cmdBegin:cmdEnd])
      elif (command == 'bearingupdate'):  
        ProcBearingUpdate(_data[cmdBegin:cmdEnd])
      elif (command == 'turningrateupdate'):  
        ProcTurningRateUpdate(_data)
      else:
        print "Unknown cmd" 
      cmdBegin = _data.find("##",cmdBegin,cmdEnd)
      cmdBegin = _data.find("!!",cmdBegin,cmdEnd)

def ProcInit(_data):
    if (debug > 0):
      print "ProcInit"
def ProcEnd(_data):
    if (debug > 0):
      print "End"
def ProcSpeedUpdate(_data):
    if (debug > 0):
      print "ProcSpeedUpdate"
    y = _data.find('speed=')
    start = y+6
    end = _data.find('##')
    print "Speed = " + _data[start:end] 
    speed = string.atof(_data[start:end]) 
def ProcBearingUpdate(_data):
    if (debug > 0):
      print "ProcBearingUpdate"+_data
    y = _data.find('bearing=')
    start = y+8
    end = _data.find('##')
    print "Bearing = " + _data[start:end] 
    bearing = string.atof(_data[start:end]) 

def ProcTurningRateUpdate(_data):
    if (debug > 0):
      print "ProcTurningRateUpdate"
    y = _data.find('turningrate=')
    start = y+12
    end = _data.find('##')
    print "TurningRate = " + _data[start:end] 
    turningRate = string.atof(_data[start:end]) 

def SendUpdate(_sock):
  print "Sending Update"
  _sock.send("!!type=update,count=2,bearing=23.5,speed=53.3,xcoord=43,ycoord=32,isalive=1,id=francis,bearing=54.2,speed=98.3,xcoord=23,ycoord=876,isalive=1,id=fletch##\n")




main()