#FORMAT python

from pyrobot.brain import Brain

class WallFollow(Brain):
    # follows walls on its left, ignores sonar sensors on its right
    def wallFollow(self, dist):
        frontLeft = self.robot.sonar[0][0].distance()
        backLeft = self.robot.sonar[0][15].distance()
        front = min([s.distance() for s in self.robot.sonar[0][2:6]])
        if front < dist:
            print "wall in front"
            self.move(0,-0.5)
        elif (frontLeft < dist or backLeft < dist):
            print "following:",
            if frontLeft < backLeft:
                print "turn slight away"
                self.move(0.3,-0.1)
            else:
                print "turn slight toward"
                self.move(0.3,0.1)
        else:
            print "find wall"
            self.move(0.3,0)
    def step(self):
        self.wallFollow(1)

def INIT(engine):
   return WallFollow('WallFollow', engine)