from bokeh.plotting import output_file, figure, show

from cbd.src.CBD import ConstantBlock, NegatorBlock, \
    IntegratorBlock, CBD, ProductBlock, AdderBlock, InverterBlock, DecisionBlock
from cbd.src.CBDDraw import draw


DELTA = 0.01

class PowerWindowUp(CBD):
    def __init__(self, blockName, delta_t = DELTA):
        CBD.__init__(self,blockName, input_ports=[], output_ports=["out_w0","out_v0", "out_Fo100","out_Fo","out_acceleration","out_acceleration_w"])
        
        self.addBlock(ConstantBlock("delta_t", value=delta_t))
        self.addBlock(ConstantBlock("torque", value=2.0))
        self.addBlock(ConstantBlock("1", value=1.0))
        self.addBlock(ConstantBlock("0.4", value=0.4))
        self.addBlock(ConstantBlock("force_threshold", value=-100))
        
        self.addBlock(ConstantBlock("in_w0", value=0.0))
        self.addBlock(ConstantBlock("in_v0", value=0.0))
        
        self.addBlock(AdderBlock("compare_force"))
        
        self.addBlock(PowerWindowPlant("plant"))
        
        self.addConnection("delta_t", "plant", input_port_name="delta_t")
        self.addConnection("torque", "plant", input_port_name="motor")
        self.addConnection("1", "plant", input_port_name="object_detect")
        self.addConnection("0.4", "plant", input_port_name="object_position")
        self.addConnection("force_threshold", "compare_force")
        
        self.addConnection("in_w0", "plant", input_port_name="init_position")
        self.addConnection("in_v0", "plant", input_port_name="init_velocity")
        
        self.addConnection("compare_force", "out_Fo100")
        
        self.addConnection("plant", "out_w0", output_port_name="position_out")
        self.addConnection("plant", "out_v0", output_port_name="velocity_out")
        self.addConnection("plant", "compare_force", output_port_name="force_out")
        self.addConnection("plant", "out_Fo", output_port_name="force_out")
        self.addConnection("plant", "out_acceleration", output_port_name="acceleration_out")
        self.addConnection("plant", "out_acceleration_w", output_port_name="acceleration_w_out")
        
        
class PowerWindowPlant(CBD):
    def __init__(self, blockName, delta_t = 0.01):
        CBD.__init__(self,blockName, input_ports=[
                                    "object_detect",
                                    "object_position",
                                    "motor", 
                                    "init_position",
                                    "init_velocity",
                                    "delta_t"
                        ], output_ports=["position_out","velocity_out","force_out","acceleration_out","acceleration_w_out"])
        
        # See power_window_object.pdf
        
        self.addBlock(ConstantBlock("m", value=10.0))
        self.addBlock(ConstantBlock("cw",value=-10.0))
        self.addBlock(ConstantBlock("zero",value=0.0))
        self.addBlock(ConstantBlock("k0",value=1000.0))
        self.addBlock(ConstantBlock("c0",value=-1000.0))
        
        self.addBlock(InverterBlock("invert_mass"))
        
        self.addBlock(NegatorBlock("position_object_neg"))
        
        self.addBlock(ProductBlock("cw_p"))
        self.addBlock(ProductBlock("divide_m"))
        self.addBlock(ProductBlock("c0_p"))
        self.addBlock(ProductBlock("k0_p"))
        self.addBlock(ProductBlock("object_detect_enable"))
        
        self.addBlock(AdderBlock("torque_friction"))
        self.addBlock(AdderBlock("total_forces"))
        self.addBlock(AdderBlock("object_compare"))
        self.addBlock(AdderBlock("object_compression"))
        self.addBlock(AdderBlock("object_displacement"))
        
        self.addBlock(IntegratorBlock("velocity"))
        self.addBlock(IntegratorBlock("position"))
        
        self.addBlock(DecisionBlock("obj_detected"))
        
        self.addConnection("object_detect", "object_detect_enable")
        self.addConnection("object_position", "position_object_neg")
        self.addConnection("motor", "torque_friction")
        self.addConnection("init_position", "position", input_port_name="IC")
        self.addConnection("init_velocity", "velocity", input_port_name="IC")
        self.addConnection("delta_t", "position", input_port_name="delta_t")
        self.addConnection("delta_t", "velocity", input_port_name="delta_t")
        
        self.addConnection("m", "invert_mass")
        self.addConnection("zero", "obj_detected", input_port_name="F")
        
        self.addConnection("invert_mass", "divide_m")
        
        self.addConnection("cw", "cw_p")
        self.addConnection("k0", "k0_p")
        self.addConnection("c0", "c0_p")
        self.addConnection("position_object_neg", "object_compare")
        self.addConnection("position_object_neg", "object_displacement")
        
        
        self.addConnection("cw_p", "torque_friction")
        self.addConnection("divide_m", "velocity")
        self.addConnection("c0_p", "object_compression")
        self.addConnection("k0_p", "object_compression")
        self.addConnection("object_detect_enable", "total_forces")
        self.addConnection("object_detect_enable", "force_out")
        
        self.addConnection("torque_friction", "total_forces")
        self.addConnection("torque_friction", "acceleration_w_out")
        self.addConnection("total_forces", "divide_m")
        self.addConnection("total_forces", "acceleration_out")
        self.addConnection("object_compare", "obj_detected", input_port_name="C")
        self.addConnection("object_compression", "obj_detected", input_port_name="T")
        self.addConnection("object_displacement", "k0_p")
        
        self.addConnection("velocity", "position")
        self.addConnection("velocity", "cw_p")
        self.addConnection("velocity", "c0_p")
        self.addConnection("velocity", "velocity_out")
        self.addConnection("position", "object_displacement")
        self.addConnection("position", "position_out")
        self.addConnection("position", "object_compare")
        
        self.addConnection("obj_detected", "object_detect_enable")
        

cbd = PowerWindowUp("powerwindow")
plantDot = draw(cbd.getBlockByName("plant"))
plantDot.render('plant.pdf')

cbd.run(int(6.0 / DELTA))

times = []
output = []

for timeValuePair in cbd.getSignal("out_w0"):
    times.append(timeValuePair.time)
    output.append(timeValuePair.value)
            
#Plot
output_file("./results.html", title="Even Numbers")
p = figure(title="Plot", x_axis_label='time', y_axis_label='N')
p.circle(x=times, y=output, legend="out_w0")
show(p)
