#!/usr/bin/env python
from bokeh.plotting import figure, output_file, show
from cbd.models.TrainControlCBD import TrainControlCBD, TrainUncontrolledCBD
from cbd.models.TrainCostModelBlock import StopSimulationException

def simulateCBD(optimalKp, optimalKi, optimalKd):
    cbd = TrainControlCBD("train_control", optimalKp, optimalKi, optimalKd)
    cbd.run(3500, 0.1)
    
    times = []
    velocity = []
    ideal_velocity = []
    displacement_person = []
    acceleration_train = []
    costs = []
    
    for timeValuePair in cbd.getBlockByName("time").getSignal("OutTime"):
        times.append(timeValuePair.value)
    
    for timeValuePair in cbd.getBlockByName("plant").getSignal("OutVTrain"):
        velocity.append(timeValuePair.value)
        
    for timeValuePair in cbd.getBlockByName("computer").getSignal():
        ideal_velocity.append(timeValuePair.value)
    
    for timeValuePair in cbd.getBlockByName("plant").getSignal("OutXPerson"):
        displacement_person.append(timeValuePair.value)
    
    for timeValuePair in cbd.getBlockByName("plant").getSignal("OutATrain"):
        acceleration_train.append(timeValuePair.value)
    
    for timeValuePair in cbd.getBlockByName("cost").getSignal("OutCost"):
        costs.append(timeValuePair.value)
    
    #Plot
    output_file("./train_velocity.html", title="Ideal and real velocity of the train")
    p = figure(title="Ideal and real velocity of the train ("+str(optimalKp)+", "+str(optimalKi)+", "+str(optimalKd)+")", x_axis_label='t', y_axis_label='m/s')
    p.line(times, ideal_velocity, legend="ideal velocity", line_width=2, line_color="red")
    p.line(times, velocity, legend="velocity", line_width=2, line_color="blue")
    show(p)
    
    output_file("./train_ideal_velocity.html", title="Ideal velocity of the train")
    p = figure(title="Ideal velocity of the train", x_axis_label='t', y_axis_label='m/s')
    p.line(times, ideal_velocity, legend="ideal velocity", line_width=2, line_color="red")
    show(p)
    
    
    #Plot
    output_file("./people_displacement.html", title="People displacement and train acceleration")
    p = figure(title="People displacement and train acceleration ("+str(optimalKp)+", "+str(optimalKi)+", "+str(optimalKd)+")", x_axis_label='t', y_axis_label='')
    p.line(times, displacement_person, legend="People displacement (m)", line_width=2, line_color="red")
    p.line(times, acceleration_train, legend="Train acceleration (m/s^2)", line_width=2, line_color="blue")
    show(p)
    
    output_file("./costs.html", title="Costs")
    p = figure(title="Costs ("+str(optimalKp)+", "+str(optimalKi)+", "+str(optimalKd)+")", x_axis_label='t', y_axis_label='')
    p.line(times, costs, legend="Cost", line_width=2, line_color="red")
    show(p)
    
    return cbd

'''
cbd = TrainControlCBD("train_control")
draw(cbd, "train_control.dot")
draw(cbd.getBlockByName("time"), "fixed_time_cbd.dot")
draw(cbd.getBlockByName("controller"), "controller.dot")
draw(cbd.getBlockByName("plant"), "plant.dot")
draw(cbd.getBlockByName("plant").getBlockByName("accelerationPerson"), "accelerationPerson.dot")
draw(cbd.getBlockByName("plant").getBlockByName("accelerationTrain"), "accelerationTrain.dot")
'''

'''
Optimal parameters list
Kp            Ki            Kd
570           100            190
490           147            183

'''

optimalKp = 570.0
optimalKps = []

optimalKi = 100.0
optimalKis = []

optimalKd = 190.0
optimalKds = []

cost = float("inf")
costs = []
for i in range(1):
    newOptimalKp = input("Kp: ")
    newOptimalKi = input("Ki: ")
    newOptimalKd = input("Kd: ")
    try:
        cbd = simulateCBD(newOptimalKp, newOptimalKi, newOptimalKd)
        
        # commit new parameters
        optimalKp = newOptimalKp
        optimalKps.append(optimalKp)
        
        optimalKi = newOptimalKi
        optimalKis.append(optimalKi)
        
        optimalKd = newOptimalKd
        optimalKds.append(optimalKd)
        
        costs.append(cbd.getBlockByName("cost").getSignal("OutCost")[-1].value)
        print("cost: " + str(costs[-1]))
    except StopSimulationException:
        print("StopSimulationException")


