import pylab
import math
import random

from sistemi import *

class G2:
    def __init__(self):
        pass
    def evaluate(self, u):
        return u + random.uniform(-0.1, 0.1)

class G3:
    def __init__(self):
      self.y = 0
      self.alpha = 0.06

    def evaluate(self, u):
      self.y = (1 - self.alpha) * self.y + self.alpha*u
      return self.y

class Reference:

    def __init__(self):
        pass

    def evaluate(self, t):
        return math.sin(t*0.5)


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 = [ ]
noisy_sensor = [ ]
filter_output = [ ]
feedback = 0

ref = Reference()
g1 = G1()
g2 = G2()
g3 = G3()
controller = PID(8, 10, 5)

while t <= 24:
    ref_signal = ref.evaluate(t)
    error = ref_signal - feedback
    controller_output = controller.evaluate(error, delta_t)
    y = g1.evaluate(controller_output, delta_t)
    sense = g2.evaluate(y)
    feedback = g3.evaluate(sense)

    ref_array.append(ref_signal)
    output.append(y)
    noisy_sensor.append(sense)
    filter_output.append(feedback)
    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.figure(2)
pylab.plot(tempi, noisy_sensor, 'b-+', label='sensor')
pylab.plot(tempi, filter_output, 'r-+', label='filter')
pylab.xlabel('time')
pylab.legend()

pylab.show()