import numpy as np
import matplotlib.pyplot as plt
import PyQt5 as qt

# Valeurs des constantes de l'exemple du cours
ptot = 1000 # [MW]
p2max = 400 # [MW]
p23max = 500
t21 = 0.4545
t22 = 0.8182

def C1(x):
    return 30*x + 0.01*x**2

def C2(x):
    return 20*x + 0.02*x**2

def f(x):
    # Lagrangien pour une optimisation simple de la production
    return C1(x[0]) + C2(x[1]) + x[2] * (ptot - x[0] - x[1])

def f2(x):
    # Lagrangien pour une optimisation avec contrainte de production max
    return f(x[0:3]) - abs(x[3]) * (p2max - x[1])

def f3(x):
    # Lagrangien pour une optimisation avec contrainte de production et de transport max
    return f(x[0:3]) - abs(x[3]) * (p23max - t21 * x[0] - t22 * x[1])

def grad(f, x, h=1e-4):
    # Il y a surement des librairies qui font ça mieux, mais c'était plus rapide d'écrire cette fonction que de chercher dans la doc
    res = []
    for i in range(len(x)):
        delta = f(x[:i] + [x[i] + h / 2] + x[i+1:]) - f(x[:i] + [x[i] - h / 2] + x[i+1:])
        res += [delta / h]
    return res

def norm(x):
    # Idem, flemme d'utiliser des arrays et de lire la doc np
    n = 0
    for d in x:
        n += d**2
    return np.sqrt(n)

def adaptation_f(f):
    # Permet de lancer la descente de gradient sur la norme du gradient du lagrangien
    l = lambda x : norm(grad(f, x, h=1e-6)) # Un pas plus faible va créer des divergences
    return l

def minimize(f, x0, h=1e-4, step=1e-1, tol=1e-8, N=1e4, echo=False):
    # Initialisation
    x = x0
    g = grad(f, x, h)
    n = 0
    prev = norm(g) + 2*tol # Très moche mais j'ai pas le temps de faire un truc plus élégant

    while abs(norm(g) - prev) > tol:
        # Mise à jour de la variable de suivi de convergence
        prev = norm(g)

        # Calcul
        for i in range(len(x)): # Moche mais flemme de rendre ça joli
            x[i] -= g[i] * step # Descente de gradient classique
        g = grad(f, x, h)

        # Print pour debug
        if (n % 100 == 0) and echo:
            print("Itération ", n)
            print("norm(g) = ", norm(g))
            print("prev = ", prev)
            print("x = ", x)
            print("g = ", g)

        # Système anti boucle infinie
        if n > N:
            return x
        n += 1
    
    return x

def custom_minimize(f, x0, echo=False):
    # Fonction avec plusieurs étapes successives de résolution, pour gagner en précision et éviter de diverger
    res_app = minimize(f, x0, step=5e-1)
    if echo:
        print(res_app)
    res_app = minimize(f, res_app, step=1e-3, tol=1e-12, h=1e-5)
    if echo:
        print(res_app)
    res_app = minimize(f, res_app, step=1e-5, tol=1e-14, h=1e-5)
    if echo:
        print(res_app)
    res_app = minimize(f, res_app, step=1e-6, tol=1e-16, h=5e-6)
    if echo:
        print(res_app)
    return res_app

print("Cas sans contraintes")
print(custom_minimize(adaptation_f(f), [0, 0, 0]))

print("Cas avec contrainte de production")
print(custom_minimize(adaptation_f(f2), [0, 0, 0, 0.01]))

print("Cas avec contraintes de production et de transport")
print(custom_minimize(adaptation_f(f3), [0, 0, 0, 0.01]))