"""Sample programs that use the librobot library.
May 2006
Nada Amin (namin@mit.edu)
"""
import librobot
import sys
import time

def ex1():
    """Simply go forward and stop when an obstacle is detected in
    front."""
    print ex1.func_doc
    robot = librobot.Robot()
    if robot.obstacle_front():
        print 'obstacle front'
    else:
        print 'no obstacle front'
        print 'moving forward'
        sys.stdout.flush()
        robot.go_forward()
        while not robot.obstacle_front():
            print '.',
            sys.stdout.flush()
        print
        print 'obstacle front'
        robot.stop()
        print 'stopping'
    print 'bye bye'
    robot.tty.close()

def ex2():
    """Avoids obstacle. Only stops when surrounded by obstacles.
    Looks and moves in the order of librobot.SENSOR_LST.
    """
    print ex2.func_doc
    robot = librobot.Robot()
    # for printing reasonable and non-redundant information
    # prev: index of the sensor without an obstacle on the previous round
    prev = -1
    # nl: whether we need a newline for pretty printing
    # (this could happen after we print a dot and then comes some text)
    nl = False
    while True:
        # stop_now: keeps track of whether there are obstacles all around
        stop_now = True
        # go through all sensors and check for obstacles
        for i in range(0, librobot.NUM_SENSORS):
            x = librobot.SENSOR_LST[i]
            if robot.obstacle(x):
                # obstacle... try the next sensor
                if prev<=i:
                    if nl:
                        print
                        nl = False
                    print 'obstacle', librobot.PRETTY_SENSORS[x]
            else:
                # no obstacle... this is where we're heading
                if prev!=i:
                    if nl:
                        print
                        nl = False
                    print 'no obstacle', librobot.PRETTY_SENSORS[x]
                    print 'going', librobot.PRETTY_DIRS[x]
                    robot.go(x)
                    prev = i
                else:
                    # same as previous round
                    print '.',
                    nl = True
                stop_now = False
                break
        # there are obstacles in all directions, so stop
        if stop_now:
            print 'Obstacles all around.'
            print 'Just stop.'
            robot.stop()
            break
        sys.stdout.flush()
    robot.tty.close()

def ex3():
    """Goes in each sensor direction until obstacle, in the order of
    librobot.SENSOR_LST.  Then stops."""
    print ex3.func_doc
    sys.stdout.flush()
    robot = librobot.Robot()
    for i in range(0, librobot.NUM_SENSORS):
        x = librobot.SENSOR_LST[i]
        if robot.obstacle(x):
            print 'obstacle', librobot.PRETTY_SENSORS[x]
            sys.stdout.flush()
            continue
        else:
            print 'going', librobot.PRETTY_DIRS[x]
            sys.stdout.flush()
            robot.go(x)
            while not robot.obstacle(x):
                print '.',
                sys.stdout.flush()
            robot.stop()
            print
            print 'obstacle', librobot.PRETTY_SENSORS[x]
            sys.stdout.flush()
    print 'Stop.'
    print 'bye bye'
    robot.tty.close()

def ex3b():
    """Goes in each sensor direction until obstacle, in the order of
    librobot.SENSOR_LST.  Then stops."""
    print ex3b.func_doc
    sys.stdout.flush()
    robot = librobot.Robot()
    for i in range(0, librobot.NUM_SENSORS):
        x = librobot.SENSOR_LST[i]
        if robot.obstacle(x):
            print 'obstacle', librobot.PRETTY_SENSORS[x]
            sys.stdout.flush()
            continue
        else:
            print 'going', librobot.PRETTY_DIRS[x]
            sys.stdout.flush()
            robot.go_until_obstacle(x, '. ')
            print
            print 'obstacle', librobot.PRETTY_SENSORS[x]
            sys.stdout.flush()
    print 'Stop.'
    print 'bye bye'
    robot.tty.close()

def ex4():
    """Follows the obstacle. If there is more than one obstacle,
    stops."""
    print ex4.func_doc
    sys.stdout.flush()
    robot = librobot.Robot()
    # prev: tracks the index of the obstacle in the previous round
    prev = -1
    # nl: tracks whether we need a newline for pretty printing
    nl = False
    while True:
        obstacles = map(lambda x: robot.obstacle(x), librobot.SENSOR_LST)
        # if there's more than one obstacle, quit
        true_obstacles = filter(lambda x: x, obstacles)
        if len(true_obstacles)>1:
            robot.stop()
            if nl:
                print
                nl = False
            print 'More than one obstacle.'
            print 'Bye bye!'
            break
        # update move according to where the obstacle occurred
        # note that i==NUM_SENSORS means that no obstacle occurred
        for i in range(0, librobot.NUM_SENSORS+1):
            if i==librobot.NUM_SENSORS or obstacles[i]:
                if prev==i:
                    # same as previous round                    
                    print '.',
                    nl = True
                else:
                    # different from previous round
                    if nl:
                        print
                        nl = False
                    if i<librobot.NUM_SENSORS:
                        x = librobot.SENSOR_LST[i]
                        print 'going', librobot.PRETTY_DIRS[x]
                        robot.go(x)
                    else:
                        print 'stopping'
                        robot.stop()
                    prev = i
                sys.stdout.flush()
                break

class DemoRobot(librobot.Robot):
    """The DemoRobot class extends librobot.Robot to perform
    sophisticated actions, requiring memory of the past."""

    def __init__(self):
        librobot.Robot.__init__(self)
        self.seq = None

    def _do_move(self, x, prev):
        """(Re-factored) Performs move x and prints appropriate
        progress information."""
        if prev!=x:
            # different from previous round
            print
            if x!=librobot.STOP:
                print 'going', librobot.PRETTY_DIRS[x]
                self.go(x)
            else:
                print 'stopping'
                self.stop()
        print '.',

    def _do_move_if_no_obstacle(self, x, prev):
        """(Re-factored) Performs move x if there is no obstacle and
        prints appropriate progress information."""
        if prev!=x:
            # different from previous round
            print
            if x!=librobot.STOP:
                print 'going', librobot.PRETTY_DIRS[x]
            else:
                print 'stopping'
        if x!=librobot.STOP and self.obstacle(x):
            self.stop()
            print 'o',
        else:
            self.go(x)
            print '.',
            
    def record_moves(self):
        """Follows the obstacle. Stops if there is more than one
        obstacle.  Records the sequence of moves thus generated."""
        # the record of moves
        self.seq = []
        # prev: tracks the move of the previous round
        prev = None
        while True:
            obstacles = map(lambda x: self.obstacle(x), librobot.SENSOR_LST)
            # if there's more than one obstacle, quit
            true_obstacles = filter(lambda x: x, obstacles)
            if len(true_obstacles)>1:
                self.stop()
                print
                print 'More than one obstacle.'
                print 'Bye bye!'
                break
            # update move according to where the obstacle occurred
            # note that i==NUM_SENSORS means that no obstacle occurred
            for i in range(0, librobot.NUM_SENSORS+1):
                if i==librobot.NUM_SENSORS or obstacles[i]:
                    if i==librobot.NUM_SENSORS:
                        x = librobot.STOP
                    else:
                        x = librobot.SENSOR_LST[i]
                    self.seq.append(x)
                    self._do_move(x, prev)
                    prev = x
                    sys.stdout.flush()
                    break

    def type_moves(self):
        """Type out the moves on the screen using f(orward),
        b(ackward), l(eft), r(ight), s(top). Any other key quits.  The
        moves are recorded as a sequence for playback."""
        self.seq = []
        prev = None
        tstart = None
        while True:
            try:
                x = raw_input("f(orward), b(ackward), r(ight), l(eft), s(top)")
            except KeyboardInterrupt:
                break
            if prev!=None:
                tend = time.time()
                tdur = int(round((tend - tstart)*10))
                self.seq += [prev]*tdur
            if x in librobot.GO_LST:
                self.go(x)
                tstart = time.time()
                prev = x
            else:
                break
        self.stop()
        print 'Bye bye!'
        
    def playback_moves(self, skipStops=False, avoidObstacles=False, curseq=None):
        """Replays the curseq or the last recorded sequence of moves."""
        if curseq==None and self.seq==None:
            print 'No sequence has been recorded.'
            return
        if curseq==None:
            curseq = self.seq
        # prev: tracks the move of the previous round
        prev = None
        for x in curseq:
            if not skipStops or x!=librobot.STOP:
                # sleep a bit to simulate the time of reading the sensors, etc.
                time.sleep(0.1)
                # update move according to the sequence of moves
                if avoidObstacles:
                    self._do_move_if_no_obstacle(x, prev)
                else:
                    self._do_move(x, prev)
                prev = x
                sys.stdout.flush()
        self.stop()
        print
        print 'Done.'
        print 'Bye bye!'

    def undo_moves(self, skipStops=False, avoidObstacles=False):
        """Undo moves of last recorded sequence, by performing both a
        time and direction reversal."""
        if self.seq==None:
            print 'No sequence has been recorded.'
            return
        rev = map(lambda x: librobot.REVERSE_DIRS[x], self.seq)
        rev.reverse()
        self.playback_moves(skipStops, avoidObstacles, rev)