import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
from sko.GA import GA


def PID_Position(P, I, D, setValue, curValue, lastErr, lastSumErr):
    Kp = P
    Ki = I
    Kd = D
    err = setValue - curValue
    derr = err - lastErr  # 此处t=1s
    sumErr = lastSumErr + err
    outPID = Kp * err + Ki * sumErr + Kd * derr
    return outPID, err, sumErr


def beControlObject(lastOutput, lastoutPID):
    output = 0.11 * lastoutPID + 0.9 * lastOutput
    return output


def ISE(err, lastISE):
    ISE = lastISE + err ** 2
    return ISE


def closeSystem(setValue, initValue, P, I, D, sumTime):
    sumErr = 0
    result = np.zeros(sumTime + 1)
    for i in range(sumTime):
        if i == 0:
            outPID, err, sumErr = PID_Position(P, I, D, setValue, initValue, 0, sumErr)
            output = beControlObject(initValue, 0)
            ISE_value = ISE(err, 0)
        else:
            lastOutPID = outPID
            outPID, err, sumErr = PID_Position(P, I, D, setValue, output, err, sumErr)
            output = beControlObject(output, lastOutPID)
            ISE_value = ISE(err, ISE_value)
        result[i + 1] = output
    return ISE_value


def resultGraphs(result, setValue, P, I, D, Time):
    setValueArray = np.ones(Time + 1) * setValue
    plt.figure(0)
    plt.plot(result)
    plt.plot(setValueArray, "r--")
    plt.title('BestContrlValue:\n P:'+str(P)+'\n I:'+str(I)+'\n D:'+str(D))
    plt.grid(True)
    plt.show()


def resultAnalysis(setValue, initValue, P, I, D, sumTime):
    sumErr = 0
    result = np.zeros(sumTime + 1)
    for i in range(sumTime):
        if i == 0:
            outPID, err, sumErr = PID_Position(P, I, D, setValue, initValue, 0, sumErr)
            output = beControlObject(initValue, 0)
            ISE_value = ISE(err, 0)
        else:
            lastOutPID = outPID
            outPID, err, sumErr = PID_Position(P, I, D, setValue, output, err, sumErr)
            output = beControlObject(output, lastOutPID)
            ISE_value = ISE(err, ISE_value)
        result[i + 1] = output
    resultGraphs(result, setValue, P, I, D, Time)


if __name__ == '__main__':
    setValue = 1
    initValue = 0
    Time = 100
    systemFunc = lambda PID: closeSystem(setValue, initValue, PID[0], PID[1], PID[2], Time)
    ga = GA(func=systemFunc, n_dim=3, size_pop=100, max_iter=500, prob_mut=0.001, lb=[1, 0.01, 0.01], ub=[20, 2, 5],
            precision=[1e-7, 1e-7, 1e-7])
    best_PID, best_ISE = ga.run()
    print('best_P:', best_PID[0], '\n','best_I:', best_PID[1], '\n','best_D:', best_PID[2], '\n', 'best_ISE:', best_ISE)

    Y_history = pd.DataFrame(ga.all_history_Y)
    fig, ax = plt.subplots(2, 1)
    ax[0].plot(Y_history.index, Y_history.values, '.', color='red')
    Y_history.min(axis=1).cummin().plot(kind='line')
    resultAnalysis(setValue, initValue, best_PID[0], best_PID[1], best_PID[2], Time)