#

# Python robot client program

#



#

# Required libraries

# socket is needed to connect to the sonar server and the ER1 server

# string is needed to send commands to the servers

#


import socket
import string
# import msvcrt

import time


HOST = '127.0.0.1'           # localhost  

ROBOTPORT = 5001             # The port used by the robot server

SONARPORT = 5010
MAPPORT   = 5005

er1Socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)  # python is cool

er1Socket.connect((HOST, ROBOTPORT))   # ditto


sonarSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)  # python is cool

sonarSocket.connect((HOST, SONARPORT))   # ditto


mapSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)  # python is cool

mapSocket.connect((HOST, MAPPORT))   # ditto


counter = 0
velocity = 10.0

posX = 0
posY = 0
posTh = 0
lastX = 0
lastY = 0
lastTh = 0
servo = 0
lastSonar = -1

servoCenter = 125
servoLeft = 240
servoRight = 30

def send(socket, command):
    socket.send(command)            # send it off to the server

    if command == "q":                   # if we want to quit

        return ""
    data = socket.recv(128)         # the server always responds!!

    lineReceived = data                  # gratuitous name change

    return lineReceived     

def sendSonar(command):
    global sonarSocket
    command += "\r\n"
    return send(sonarSocket, command)

def sendMap(command):
    global mapSocket
    command += "\r\n"
    send(mapSocket, command)

def sendRobot(command):
    global er1Socket
    return send(er1Socket, command)

def updatePos():
    print "updatePos"
    global posX
    global posY
    global posTh
    global lastX
    global lastY
    global lastTh

    lastX = posX;
    lastY = posY;
    lastTh = posTh;

    odo = sendRobot("p")
    odostuff = odo.split(' ')
    posX = float(odostuff[0])
    posY = float(odostuff[1])
    posTh = float(odostuff[2])    
    sendMap("d " + str(posX - lastX) + " " + str(posY - lastY) + " " + str(posTh - lastTh))

def sendSonarToMap(val):
    print "sendSonarToMap"
    global servo
    sendMap("s " + str(val) + " " + str(servo))


def updateParticles():
    print "updateParticles"
    sendMap("p")         # Turn particles off

    sendMap("m")
    time.sleep(0.1)
    sendMap("n")
    time.sleep(0.1)
    sendMap("b")
    sendMap("p")         # Turn particles on


def doMcl():
    val = int(sendSonar("1"))
    print "got sonar"
    updatePos()
    time.sleep(0.1)
    sendSonarToMap(val)
    time.sleep(0.1)
    updateParticles()
    lastSonar = val
    return val

sendMap("p");     # turn particles off


while 1:
    # note that whitespace and indentation matters in python

    # feel free to write a java, C++, perl, or befunge client...

    print 'Please input a line to send'     # test commands by hand

    lineToSend = raw_input()                # get input 

    if lineToSend == "mcl":
        print "Doing mcl";
        doMcl();
        continue;
    er1Socket.send(lineToSend);                  # send it off to the server

    if lineToSend == "q":                   # if we want to quit

        break;
    data = er1Socket.recv(128);                  # the server always responds!!

    lineReceived = data                     # gratuitous name change

    print 'Received', lineReceived          
    if lineReceived[0] == 'q':              # if server is tired of

        break;                              # getting bullied around...



sonarSocket.close()
time.sleep(0.1)
mapSocket.close()
time.sleep(0.1)
er1Socket.close()