#FORMAT python

# OUT_OF_DATE! THIS USES THE OLD PLAYER INTERNAL BLOB DATA STRUCTURES

from pyrobot.brain import Brain

# Seeks out red blobs
class FindBlobs(Brain):
    def setup(self):
        self.startDevice('blobfinder')

    def destroy(self):
        self.removeDevice('blobfinder')

    def step(self):
        self.seekColor('red')

    def seekColor(self, color):
        minRange = 0.9
        if self.collisionImminent('front-all', minRange):
            translate, rotate = self.avoid(minRange)
            self.move(translate, rotate)
        else:
            result = self.getLargestBlob(color)
            if result == None:
                leftSide = min([s.distance() for s in self.robot.range["front-left"]])
                rightSide = min([s.distance() for s in self.robot.range["front-right"]])
                if leftSide < rightSide:
                    self.move(0.1, -0.5)
                else:
                    self.move(0.1, 0.5)
            else:
                (turnDirection, distance) = result
                if turnDirection > 0:
                    self.move(0.1, 0.15)
                elif turnDirection < 0:
                    self.move(0.1, -0.15)
                else:
                    self.move(0.2, 0)

    # Returns True when the minimum sonar value from the given location
    # group is less than the minRange.
    def collisionImminent(self, location, minRange):
        return min([s.distance() for s in self.robot.range[location]]) < minRange

    # Returns translate and rotate values for avoiding obstacles.
    def avoid(self, minRange):
        if self.collisionImminent('front', minRange):
            return 0, -0.3
        elif self.collisionImminent('front-left', minRange):
            return 0, -0.3
        elif self.collisionImminent('front-right', minRange):
            return 0, 0.3
        else:
            return 0.2, 0

    # Returns a list of all blobs of the given color.
    def getBlobs(self, color):
        data = self.robot.blobfinder.data
        (width, height) = data[0]
        colorChannels = data[1]
        if color == 'red':
           index = 0
        elif color == 'green':
           index = 1
        elif color == 'blue':
           index = 2
        else:
           raise ValueError, 'unrecognized color channel: %s' % color
        blobs = colorChannels[index]
        return blobs

    # Returns the direction (+1, -1, or 0) to the largest blob of the
    # specified color along with the distance to the blob.  The blob
    # information consists of a list of nine features:
    #     (colorCode, area, xc, yc, x0, x1, y0, y1, distance)
    # xc is an integer between 0 and 160 which represents the x position of
    # the blob's center.  A value of 0 indicates that the blob is at the
    # farthest left location, a value of 80 indicates that the blob is
    # centered, and a value of 160 indicates that the blob is at the farthest
    # right location.  Distance is an integer representing the distance to the
    # blob.  When using a Pioneer robot with a gripper, if the blob is
    # centered, then a range of 380 indicates that it is within the grasp of
    # the gripper.
    def getLargestBlob(self, color):
        allBlobs = self.getBlobs(color)
        if len(allBlobs) == 0:
            return None
        else:
            largestBlob = allBlobs[0]
            (colorCode, area, xc, yc, x0, x1, y0, y1, distance) = largestBlob
            if xc < 75:
                turnDirection = +1
            elif xc > 85:
                turnDirection = -1
            else:
                turnDirection = 0
            return (turnDirection, distance)


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