import pylab
import math

from sistemi import *

class Reference:

    def __init__(self):
        pass

    def evaluate(self, t):
        if t < 2:
            return 3*t
        elif t < 10:
            return 6
        elif t < 13:
            return 6 - 2 * (t - 10)
        else:
            return 0


class PID:

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

    def evaluate(self, error, delta_t):
        deriv = (error - self.prev_error) / delta_t
        self.prev_error = error
        self.integral = self.integral + error * delta_t
        output = self.kp * error + self.ki * self.integral + self.kd * deriv
        return output


t = 0.0
delta_t = 1e-3

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

ref = Reference()
sistema = BigSystem()
controller = PID(6, 8, 15)

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, ref_array, 'b-+', label='input')
pylab.plot(tempi, output, 'r-+', label='output')
pylab.xlabel('time')
pylab.legend()

pylab.show()