# A Subsumption Behavior-based Brain
# A starting point for finding light in an environment with obstacles.
# Layer 0: Wander
# Layer 1: SeekLight
# Layer 2: FoundLight
# Layer 3: AvoidObstacles

from pyrobot.brain import Brain
import time, random

class SubsumptionBrain(Brain):

    def setup(self):
        self.robot.light[0].units = 'SCALED'
        self.behaviors = []
        # add behaviors, highest priorities first
        self.addBehavior(AvoidObstacles())
        self.addBehavior(FoundLight())
        self.addBehavior(SeekLight())
        self.addBehavior(Wander())

    def addBehavior(self, behavior):
        self.behaviors.append(behavior)
        behavior.robot = self.robot

    def step(self):
        behavior = self.updateAll()
        print '%s is in control' % behavior
        self.robot.move(behavior.speed, behavior.rotate)
        time.sleep(1)

    def updateAll(self):
        # update all behaviors in order of priority
        for behavior in self.behaviors:
            behavior.flag = False
            behavior.update()
            # if the behavior fired, return it immediately
            if behavior.flag:
                return behavior
        # if none fired, return the lowest-priority (last) behavior
        return self.behaviors[-1]

#------------------------------------------------------------------
# behaviors

class SubsumptionBehavior:
    def __init__(self):
        self.speed = 0
        self.rotate = 0
        self.flag = False
        self.robot = None   # this will be set by addBehavior

    def __str__(self):
        return self.__class__.__name__

    def requestMove(self, speed, rotate):
        self.speed = speed
        self.rotate = rotate
        self.flag = True


class Wander(SubsumptionBehavior):
    def update(self):
        self.requestMove(0.3, random.uniform(-0.5, 0.5))


class SeekLight(SubsumptionBehavior):
    def update(self):
        if max([s.value for s in self.robot.light[0]['all']]) > 0:
            left = max([s.value for s in self.robot.light[0]['left']])
            right = max([s.value for s in self.robot.light[0]['right']])
            rotation = left - right
            if left > right:
                self.requestMove(0.2, rotation + 0.1)
            else:
                self.requestMove(0.2, rotation - 0.1)


class FoundLight(SubsumptionBehavior):
    def update(self):
        if max([s.value for s in self.robot.light[0]['all']]) > 0.9:
            self.requestMove(0, 0)


class AvoidObstacles(SubsumptionBehavior):
    def update(self):
        if min([s.distance() for s in self.robot.range['front-all']]) < 1:
            frontLeft= min([s.distance() for s in self.robot.range['front-left']])
            frontRight= min([s.distance() for s in self.robot.range['front-right']])
            if frontLeft < frontRight:
                self.requestMove(0.1, -1.0 + frontLeft)
            else:
                self.requestMove(0.1, 1.0 - frontRight) 

#------------------------------------------------------------------

def INIT(engine):
    return SubsumptionBrain('Subsumption Brain', engine)