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

# Vn = 4e5 # En V

def make_Y(n):
    Y = np.zeros((n, n))
    return Y

def connect_Y(x, y, Ys, Yp, Y):
    Y[x, y] = -Ys
    Y[y, x] = -Ys
    Y[x, x] += Ys + Yp
    Y[y, y] += Ys + Yp

def spec(n, Y, Vn):
    S = np.zeros((n, n))
    for i in range(n):
        for k in range(n):
            if i == k:
                S[i, k] = n
            else:
                S[i, k] = -1
            S[i, k] *= Vn**2 * Y[i, k]
    return S

def delta_select(i, S):
    S = np.delete(S, (i), axis=0)
    S = np.delete(S, (i), axis=1)
    return S

def power_select(i, P):
    P = np.array(P[:i].tolist() + P[i+1:].tolist())
    return P

def complete_data(P, delta, i):
    ndelta = np.array(delta[:i].tolist() + [0] + delta[i:].tolist())
    nP = np.array(P[:i].tolist() + [-np.sum(P)] + P[i:].tolist())
    return ndelta, nP

# Vecteur des puissances
P = np.array([1000, -500, -250, -250])
P = P * 1e6 # Passage en MW

# Création de la matrice d'admitances (dimension n)
Y = make_Y(4)
connect_Y(2, 3, 0.1, 0, Y)
connect_Y(1, 3, 0.15, 0, Y)
connect_Y(2, 1, 0.05, 0, Y)
connect_Y(2, 0, 0.05, 0, Y)
connect_Y(3, 0, 0.05, 0, Y)
print("Admittance matrix :", Y)

# Mise en place du système linéaire à résoudre
S = spec(4, Y, 2e5) # dim n
S = delta_select(3, S) # dim n-1, sélection de l'angle de transport de référence (delta_3)
print("System matrix :", S)

# Sélection des puissances (dimension n-1)
P = power_select(3, P)
print("Puissances de référence : ", P)

# Résolution (dimension n-1)
invS = np.linalg.inv(S)
print("Inverse : ", invS)

# Calcul des angles de transport (dimension n-1)
delta = np.dot(invS, P)

# Ajout de l'angle de transport d'origine et de la puissance associée (on repasse en dim n)
ndelta, nP = complete_data(P, delta, 3)
print("Power input :", nP)
print("Delta (rad) :", ndelta * 180 / 3.1415)