import pylab
import math

from sistemi import *

class Reference:

    def __init__(self):
        pass

    def evaluate(self, t):
        if t < 4:
            return math.sqrt(t)
        else:
            return 2


class PI:

    def __init__(self, kp, ki):
        self.kp = kp
        self.ki = ki
        self.integral = 0

    def evaluate(self, error, delta_t):
        self.integral = self.integral + error * delta_t
        output = self.kp * error + self.ki * self.integral
        return output


t = 0.0
delta_t = 1e-3 

tempi = [ ]
output = [ ]
ref_array = [ ]
y = 0

ref = Reference()
sistema = BigSystem()
controller = PI(28, 2)

while t <= 24:
    ref_signal = ref.evaluate(t)
    error = ref_signal - y
    controller_output = controller.evaluate(error, delta_t)
    y = sistema.evaluate(controller_output, delta_t)

    ref_array.append(ref_signal)
    output.append(y)
    tempi.append(t)

    t = t + delta_t


pylab.figure(1)
pylab.plot(tempi, output, 'r-+', label='output')
pylab.plot(tempi, ref_array, 'b-+', label='input')
pylab.xlabel('time')
pylab.legend()

pylab.show()