#FORMAT python

from pyrobot.brain.fuzzy import *
from pyrobot.brain.behaviors import *
from phissioncamera import *
from time import sleep

class BBB(BehaviorBasedBrain):
    def destroy(self):
        self.removeDevice("ptz0")

class Avoid (Behavior):
    """Avoid Class"""
    def update(self):
        if( camera.getMaxBlob_mass() > 200 ):
            self.IF(Fuzzy(0, 160 ) << camera.getMaxBlob_cx(), 'pan',  5.0, "pan left")
            self.IF(Fuzzy(0, 160 ) >> camera.getMaxBlob_cx(), 'pan', -5.0, "pan right")
            self.IF(Fuzzy(0, 120 ) << camera.getMaxBlob_cy(), 'tilt',-5.0, "tilt down")
            self.IF(Fuzzy(0, 120 ) >> camera.getMaxBlob_cy(), 'tilt', 5.0, "tilt up")

class state1 (State):
    """ sample state """
    def setup(self):
        print "about to train color"
        sleep(10)
        camera.SpotColorTrackHSV_Filter(80,60,8, (20, 80,110))
        print "color trained!"
        self.add(Avoid(1, {'pan': 1, 'tilt': 1}))

def INIT(engine):
    engine.robot.startDevice("ptz")
    ptz = engine.robot.ptz[0]
    brain = BBB({'pan' : ptz.panRel,
                 'tilt' : ptz.tiltRel,
                 'update' : engine.robot.update }, engine)
    brain.camera = phissioncamera()
    brain.camera.setup(0,160,120);
    brain.add(state1())
    brain.activate('state1')
    return brain