from pyrobot.brain import Brain

class WallFollowBrain(Brain):

    def step(self):
        # maintain this distance between robot and walls (in robot units)
        self.clearance = 0.5
        (speed, rotate) = self.wallFollow()
        self.robot.move(speed, rotate)

    # follows walls on its left, ignores sonar sensors on its right
    def wallFollow(self):
        frontLeft = self.robot.range[0].distance()
        backLeft = self.robot.range[15].distance()
        frontMin = min([s.distance() for s in self.robot.range[2:6]])
        if frontMin < self.clearance:
            print "wall in front"
            return (0, -0.5)
        elif (frontLeft < self.clearance or backLeft < self.clearance):
            print "following:",
            if frontLeft < backLeft:
                print "turning slightly away"
                return (0.3, -0.1)
            else:
                print "turning slightly toward"
                return (0.3, 0.1)
        else:
            print "looking for wall"
            return (0.3, 0)


def INIT(engine):
   return WallFollowBrain('Wall Follower', engine)