"""Python Library for 6.115 Final Project
May 2006
Nada Amin (namin@mit.edu)

This library interfaces with the R31JP using the serial port and
commands the robot to move around or to send sensor information.
"""
import serial
import sys

# constants for both directions and sensors
BACK = 'b'
FRONT = 'f'
LEFT = 'l'
RIGHT = 'r'
STOP = 's'

# list of all possible constants for direction commands
GO_LST = [FRONT, LEFT, RIGHT, BACK, STOP]
# table that maps a constant to a direction command
GO_TABLE = {FRONT : 'F', BACK : 'B', LEFT : 'L', RIGHT : 'K', STOP : 'H'}

# table that maps a direction to constant to its opposite
REVERSE_DIRS = {FRONT : BACK, BACK : FRONT, LEFT : RIGHT, RIGHT : LEFT, STOP : STOP}

# list of all possible constants for sensory position
SENSOR_LST = [FRONT, LEFT, RIGHT, BACK]
# table that maps a constant to a sensory position
SENSOR_TABLE = {FRONT : '0', BACK : '1', LEFT: '2', RIGHT : '3'}

PRETTY_SENSORS = {FRONT : 'front', LEFT: 'left', RIGHT : 'right', BACK: 'back'}
PRETTY_DIRS = {FRONT: 'forward', LEFT: 'left', RIGHT: 'right', BACK: 'backward'}

NUM_SENSORS = len(SENSOR_LST)

class Robot:
    """The Robot class defines the basic library interface to the
    R31JP-controlled robot."""
    def __init__(self):
        """Initializes the serial port (will propage the errors from
        the serial library)."""
        self.tty = serial.Serial(0)

    def _ttyread(self):
        """Waits for a message on the serial port and returns it.
        Note that this procedure is not robust, because it might wait
        forever if no message comes.
        """
        while self.tty.inWaiting()==0:
            pass
        return self.tty.read(self.tty.inWaiting())

    def go(self, x):
        """Commands robot to go in the given direction (or stop)."""
        self.tty.write(GO_TABLE[x])
        out = self._ttyread()
        return out == GO_TABLE[x]+'\r\n*'
    
    def go_forward(self):
        """Commands the robot to go forward."""
        return self.go(FRONT)

    def go_backward(self):
        """Commands the robot to go backward."""
        return self.go(BACK)
    
    def go_left(self):
        """Commands the robot to go left."""
        return self.go(LEFT)
    
    def go_right(self):
        """Commands the robot to go right."""
        return self.go(RIGHT)
    
    def stop(self):
        """Commands the robot to stop."""
        return self.go(STOP)
    
    def sensor(self, x):
        """Returns the sensory value for the given sensor position."""
        self.tty.write('s'+SENSOR_TABLE[x])
        out = self._ttyread()
        val = out[-5:-3]
        return int('0x'+val, 16)

    def obstacle(self, x):
        """Returns whether there is an obstacle at the given sensor
        position."""
        return self.sensor(x) >= 80

    def obstacle_front(self):
        """Returns whether there is an obstacle on the front."""
        return self.obstacle(FRONT)

    def obstacle_back(self):
        """Returns whether there is an obstacle on the back."""
        return self.obstacle(BACK)
    
    def obstacle_left(self):
        """Returns whether there is an obstacle on the left."""
        return self.obstacle(LEFT)
    
    def obstacle_right(self):
        """Returns whether there is an obstacle on the right."""
        return self.obstacle(RIGHT)

    def go_until_obstacle(self, x, txt=''):
        """Goes in the given direction, until the sensor in that
        direction detects an obstacle, and stops. txt is an optional
        message to be printed at every round.
        """
        self.go(x)
        while not self.obstacle(x):
            sys.stdout.write(txt)
            sys.stdout.flush()
        self.stop()
        
    def go_forward_until_obstacle(self, txt=''):
        """Moves forward until there is an obstacle on the front, then
        stops."""
        self.go_until_obstacle(FRONT, txt)

    def go_backward_until_obstacle(self, txt=''):
        """Moves backward until there is an obstacle on the back, then
        stops."""
        self.go_until_obstacle(BACK, txt)

    def go_left_until_obstacle(self, txt=''):
        """Moves left until there is an obstacle on the left, then
        stops."""
        self.go_until_obstacle(LEFT, txt)

    def go_right_until_obstacle(self, txt=''):
        """Moves right until there is an obstacle on the right, then
        stops."""
        self.go_until_obstacle(RIGHT, txt)