from CBDMultipleOutput.Source.CBD import CBD, ConstantBlock, \
    AdderBlock, BaseBlock, ProductBlock, IntegratorBlock
from cbd_tfsa_woven_classes import FSABlock, WhenVarCrossesThresholdTrigger,\
    ValuedState
from fsaclasses import FSAModel, Transition, After, Float

class PowerWindowCBD(CBD):
    def __init__(self, blockName, delta_t = 0.01):
        CBD.__init__(self,blockName, input_ports=[], output_ports=[])
        self.addBlock(ConstantBlock("delta", value=delta_t))
        self.addBlock(Environment("environment"))
        self.addBlock(InterfaceFSA("controller"))
        self.addBlock(PowerWindowPlant("plant"))
        
        self.addConnection("environment", "controller", 
                           input_port_name="force", output_port_name="force")
        self.addConnection("environment", "controller", 
                           input_port_name="down", output_port_name="down")
        self.addConnection("environment", "controller", 
                           input_port_name="up", output_port_name="up")
        
        self.addConnection("controller", "plant", 
                           input_port_name="motor", output_port_name="motor")
        self.addConnection("delta", "plant", 
                           input_port_name="delta_t")
        
    def setDeltaT(self, deltaT):
        CBD.setDeltaT(self, deltaT)
        self.getBlockByName("delta").setValue(deltaT);

class Environment(BaseBlock):
    def __init__(self, block_name):
        BaseBlock.__init__(self, block_name, [], ["force","down","up"])
        self.__values = []
    
    def compute(self, curIteration):
        time = self.getClock().getTime()
        
        force_value = 0.0
        down_value = 0.0
        up_value = 0.0
        
        if time < 6.0:
            up_value = 1.0
        elif time < 7.0:
            up_value = 0.0
        else:
            up_value = 1.0
        
        if time < 4.0:
            force_value = 0.0
        elif time < 4.5:
            force_value = 150.0
        else:
            force_value = 0.0
        
        self.appendToSignal(force_value, "force")
        self.appendToSignal(down_value, "down")
        self.appendToSignal(up_value, "up")

class InterfaceFSA(FSABlock):
    def __init__(self, name):
        
        s_down = ValuedState("down", -1.0)
        s_neutral = ValuedState("neutral", 0.0)
        s_up = ValuedState("up", 1.0)
        s_emergency = ValuedState("emergency", -1.0)
        
        t_neutral_down = Transition("neutral_down",s_neutral,s_down)
        t_neutral_down.trigger = WhenVarCrossesThresholdTrigger("down", 0.5, ">")
        t_down_neutral = Transition("down_neutral",s_down, s_neutral)
        t_down_neutral.trigger = WhenVarCrossesThresholdTrigger("down", 0.5, "<")
        
        t_neutral_up = Transition("neutral_up",s_neutral, s_up)
        t_neutral_up.trigger = WhenVarCrossesThresholdTrigger("up", 0.5, ">")
        t_up_neutral = Transition("up_neutral",s_up, s_neutral)
        t_up_neutral.trigger = WhenVarCrossesThresholdTrigger("up", 0.5, "<")
        
        t_up_emergency = Transition("up_emergency",s_up, s_emergency)
        t_up_emergency.trigger = WhenVarCrossesThresholdTrigger("force", 100, ">")
        
        t_emergency_neutral = Transition("emergency_neutral",s_emergency, s_neutral)
        t_emergency_neutral.trigger = After(Float(1.0))
        
        fsaModel = FSAModel(
                        [s_down,s_neutral,s_up,s_emergency],
                        [t_neutral_down,t_down_neutral,
                         t_neutral_up,t_up_neutral,
                         t_up_emergency,t_emergency_neutral]
                        )
        fsaModel.initialState = s_neutral
        
        FSABlock.__init__(self, name, fsaModel, ["down", "up", "force"], ["motor"])


class PowerWindowPlant(CBD):
    def __init__(self, blockName, delta_t = 0.01):
        CBD.__init__(self,blockName, input_ports=["motor", "delta_t"], output_ports=["position"])
        self.addBlock(ConstantBlock("180", value=180.0))
        self.addBlock(ProductBlock("times_180"))
        self.addConnection("motor", "times_180")
        self.addConnection("180", "times_180")
        self.addBlock(AdderBlock("plus"))
        self.addConnection("times_180", "plus")
        self.addBlock(ConstantBlock("zero", value=0.0))
        self.addBlock(IntegratorBlock("velocity"))
        self.addConnection("zero", "velocity", input_port_name="IC")
        self.addConnection("delta_t", "velocity", input_port_name="delta_t")
        self.addConnection("plus", "velocity")
        self.addBlock(ConstantBlock("minus10", value=-10.0))
        self.addBlock(ProductBlock("times_minus_10"))
        self.addConnection("minus10", "times_minus_10")
        self.addConnection("velocity", "times_minus_10")
        self.addConnection("times_minus_10", "plus")
        self.addBlock(IntegratorBlock("pos"))
        self.addConnection("zero", "pos", input_port_name="IC")
        self.addConnection("delta_t", "pos", input_port_name="delta_t")
        self.addConnection("velocity", "pos")
        self.addConnection("pos", "position")
    