# 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: Avoid

from pyrobot.brain import Brain
import time
import random

class SubsumptionBehavior:
    def __init__(self):
        self.translate = 0
        self.rotate = 0
        self.flag = 0

    def setRobot(self, robot):
        self.robot = robot

    def move(self, translate, rotate):
        self.translate = translate
        self.rotate = rotate
        self.flag = 1

class SubsumptionBrain(Brain):
    def __init__(self, engine):
        Brain.__init__(self, 'SubsumptionBrain', engine)
        self.behaviors = []
        self.robot = engine.robot
        self.robot.light[0].units = "SCALED"

    def add(self, behavior):
        behavior.setRobot( self.robot )
        self.behaviors.append( behavior )

    def step(self):
        b = self.updateAll()
        print "%s is in control" % self.behaviors[b].__class__.__name__
        self.move(self.behaviors[b].translate,
                  self.behaviors[b].rotate)
        time.sleep(1)

    def updateAll(self):
        # for all except lowest:
        for b in range(len(self.behaviors) - 1, 0, -1):
            self.behaviors[b].flag = 0
            self.behaviors[b].update()
            # if it fired, return number:
            if self.behaviors[b].flag:
                return b
        # if none fired, return lowest:
        self.behaviors[0].update()
        return 0

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

class Wander(SubsumptionBehavior):
    def update(self):
        self.move( .3, random.random() - 0.5)

class SeekLight(SubsumptionBehavior):
    def update(self):
        if  max([s.value for s in self.robot.light[0]["all"]]) > 0.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.move(0.1, rotation + 0.1)
            else:
                self.move(0.1, rotation - 0.1)

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

class Avoid(SubsumptionBehavior):
    def update(self):
        if min([s.value for s in self.robot.range["front-all"]]) < 1.0:
            frontLeft= min([s.value for s in self.robot.range["front-left"]])
            frontRight= min([s.value for s in self.robot.range["front-right"]])
            if frontLeft < frontRight:
                self.move(0.05, -1.0 + frontLeft)
            else:
                self.move(0.05, 1.0 - frontRight) 

def INIT(engine):
    subsumption = SubsumptionBrain( engine )
    # add behaviors, lowest priorities first:
    subsumption.add( Wander() )
    subsumption.add( SeekLight() )
    subsumption.add( FoundLight() )
    subsumption.add( Avoid() )
    return subsumption