import math
import matplotlib.pyplot as plt
def Nalej(tp, a, beta, setpoint, no_of_steps):
        Kp = 5
        Td = 0.2
        Ti = 0.2

        data = []
        data_to_second = []
        result = 10 #h0 - poziom przy starcie regulacji
        errorList = []
        for i in range(no_of_steps):
            error = setpoint - result
            errorList.append(error)
            sumOfError = sum(errorList)
            result = tp * ((1 / a) * PID(Kp, Td, Ti, 0.7, error, sumOfError, errorList[-1]) - (beta / a) * math.sqrt(result)) + result
            data.append(result)

        result = 0;
        for i in range(no_of_steps):
            error = setpoint - result
            errorList.append(error)
            sumOfError = sum(errorList)
            result = tp * ((1 / a) * PID(0.5*Kp, 0.5*Td, 0.5*Ti, 0.7, error, sumOfError, errorList[-1]) - (beta / a) * math.sqrt(result)) + result
            data_to_second.append(result)

        plt.plot(data,label='Nastawy')
       # plt.plot(data_to_second,label='Nastawy/2')
        plt.legend(loc='best')
        plt.title('Nastawy: P: %s | I: %s | D: %s ' % (Kp,Td,Ti))
        plt.axhline(setpoint,linewidth=0.5,color='r')
        plt.show()

def PID(kp, td, ti, tp, error,errorSum, prevError):
    signal = kp * ((error + (tp / ti) * errorSum + (td/ti) * (error - prevError)))
    return signal * 0.1

Nalej(0.2, 2, 0.25, 1, 100)