"""
This program contains several functions for frequency-domain stability analysis including:
    1) Generalized Nyquist Criterion (GNC) application to determine system stability: via eigenvalue decomposition and via the determinant
    2) Eigenvalue Decomposition (EVD) of the closed-loop matrix to determine oscillatory modes and bus participation factors
    3) Passivity index (for the application of the passivity theorem) and singular value decomposition (small-gain theorem) of target matrices
    4) A main stability_analysis function to apply all the previously described to a specific system

Copyright (C) 2026  Francisco Javier Cifuentes Garcia

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <https://www.gnu.org/licenses/>.
"""

__all__ = ['stability_analysis','passivity','nyquist','small_gain','EVD','nyquist_det']

import numpy as np
from scipy.optimize import linear_sum_assignment
import matplotlib.pyplot as plt
from .read_admittance import read_admittance
from os import path, makedirs
import pickle

from matplotlib import rcParams  # Text's parameters for plots
rcParams['mathtext.fontset'] = 'cm'  # Font selection
rcParams['font.family'] = 'STIXGeneral'  # 'cmu serif'

class Graph:
    def __init__(self, V):
        self.V = V
        self.adj = [[] for i in range(V)]

    # Depth-first search algorithm method
    def DFSUtil(self, temp, v, visited):
        # Mark the current vertex as visited
        visited[v] = True
        # Add the vertex to list
        temp.append(v)
        # Repeat for all vertices adjacent to this vertex v
        for i in self.adj[v]:
            if not visited[i]:
                # Update the list
                temp = self.DFSUtil(temp, i, visited)
        return temp

    # Add an undirected edge
    def addEdge(self, v, w):
        self.adj[v].append(w)
        self.adj[w].append(v)

    # Method to retrieve connected components in an undirected graph
    def connectedComponents(self):
        visited = []
        cc = []
        for i in range(self.V):
            visited.append(False)
        for v in range(self.V):
            if not visited[v]:
                temp = []
                cc.append(self.DFSUtil(temp, v, visited))
        return cc

def stability_analysis(topology=None, results_folder=None, file_root=None, check_conditioning=False, condition_number_th=10e6, make_plot=True, indentations=[], save_pickle=False):
    # 0) Read the terminal angle information
    block_area_angle = []  # List for each block containing a list as [bus/block name, area_id, terminal angle in rad]
    with open(results_folder+r'\\'+file_root+'_angles.txt', 'r') as file:
        next(file)  # First line contains the header
        for line in file:
            content = [str(line.strip().split("\t")[0]),int(line.strip().split("\t")[1]),float(line.strip().split("\t")[2])]
            block_area_angle.append(content)
    areas = set([bus[1] for bus in block_area_angle])
    # Define a reference bus for each area
    reference_angle = {i: -100.0 for i in areas}  # Unrealistic intial values
    for bus in block_area_angle:
        if len(areas) == 1:
            reference_angle[0] = float(bus[2])
            reference_angle[1] = float(bus[2])
        else:
            if reference_angle[bus[1]] == -100.0:
                reference_angle[bus[1]] = float(bus[2])  # If no angle is already defined, use the first one from that area
                # print("The reference for area",bus[1],'is bus',bus[0])

    # This function loads and builds the edge and nodal admittance matrices and for stability analysis
    # 1) Read the topology matrix and extract block names
    Ytopology = np.loadtxt(topology, skiprows=1, comments=["#", "%", "!"])
    # nameA-1 nameA-2 nameB-1 nameB-2 ... x nameA-1 nameA-2 nameB-1 ...
    # 0 means no interconnection, 1 means connection between the edges: diagonals are single-sided / shunt
    with open(topology, 'r') as f:
        block_names_Y = f.readline().strip('\n').split("\t")

    # 2) Read the admittance files based on the topology file
    admittances = []  # List containing the admittance objects
    bus_names = []  # List of list with block (bus) names
    # Create the undirected graph: adjacent matrix but diagonals can be 1
    g = Graph(len(Ytopology))
    for row, name in enumerate(block_names_Y):
        for col, edge in enumerate(Ytopology[row]):
            if int(edge) == 1: g.addEdge(row, col)
    # Obtain the connected components of the graph
    cc = g.connectedComponents()  # List of lists with blocks positions connected
    # For every group of connected buses, read and add the admittance matrix
    for buses in cc:
        bus_names.append([block_names_Y[bus] for bus in buses])
        admittances.append(read_admittance(path=results_folder, involved_blocks=bus_names[-1], file_root=file_root))

    # 3) Update bus_names to be ordered as the variables in the individual admittance matrices and build the node matrix
    node_matrix = []  # Create the node matrix with the active components (block diagonal)
    node_variables = []  # List of variable names = the current/voltage vectors
    edge_aux_matrix = []  # Auxiliary edge matrix (block diagonal)
    edge_aux_variables = []  # The order of this aux matrix is different from the order of the node matrix
    for idx, y in enumerate(admittances):
        bus_names[idx] = y.blocks
        # print(bus_names[idx],y.y_type,"- Node:",y.node)
        if y.node:
            # Define the nodal matrix that sets the order of the electrical variables
            for var in y.vars: node_variables.append(var)
            node_matrix.append(y)
        else:
            # Update the aux edge matrix and its variables
            for var in y.vars: edge_aux_variables.append(var)
            edge_aux_matrix.append(y)

    edge_ordering = []  # List to re-sort the edge matrix acording to the node matrix variables
    for var in node_variables: edge_ordering.append(edge_aux_variables.index(var))
    # print("Node vars \n",node_variables,"\nEdge vars \n",edge_aux_variables)
    # print("\nSorted edge variables \n",sorted(edge_aux_variables,key=node_variables.index))

    # 4) Create the node and edge matrices with the frequency domain data
    frequencies = admittances[0].f  # Retreive the frequency vector
    # Create the auxiliary edge matrix (different order than node matrix), useful to check the network topology and scan
    Yedge_aux = np.zeros((len(frequencies),len(node_variables),len(node_variables)),dtype='cdouble')  # Or dtype='csingle'
    y_edge_idx = 0
    for idx, yedge in enumerate(edge_aux_matrix):
        # # Eliminate too small elements: related to PSCAD accuracy when the topology is not enforced
        # for col in range(np.size(yedge.y, 1)):
        #     for row in range(np.size(yedge.y, 2)):
        #         if max(abs(yedge.y[:, row, col])) < 1e-6:  # The threshold is system and time-step dependent
        #             yedge.y[:, row, col] = np.zeros(yedge.y[:, row, col].shape)
        Yedge_aux[:, y_edge_idx:y_edge_idx + len(yedge.vars), y_edge_idx:y_edge_idx + len(yedge.vars)] = yedge.y
        y_edge_idx = y_edge_idx + len(yedge.vars)  # Update the matrix index for the next admittance block

    # Sparsity plot for verification at the lowest frequency
    np.seterr(divide='ignore')
    Yedge_aux_nan = np.zeros_like(Yedge_aux[0,:,:], dtype=np.float64)
    Yedge_aux_nan.fill(np.nan)
    plt.imshow(20*np.log10(np.abs(Yedge_aux[0,:,:]), out=Yedge_aux_nan, where=(np.abs(Yedge_aux[0, :, :])!=0.0)),cmap='spring', interpolation='nearest')
    plt.colorbar()
    plt.xticks(ticks=np.arange(0, len(edge_aux_variables), step=1), labels=[])
    plt.yticks(ticks=np.arange(0, len(edge_aux_variables), step=1), labels=edge_aux_variables)
    plt.grid(visible=True, which='minor', alpha=0.3, color='k', linestyle='-', linewidth=0.5)
    plt.title('Auxiliary edge admittance matrix at '+format(frequencies[0], '.1f')+' Hz')
    plt.savefig(results_folder + '\\' + file_root + "_Edge_aux.pdf", format="pdf", bbox_inches="tight")
    # with open(results_folder + '\\' + file_root + "_Edge_aux.pickle", 'wb') as f: pickle.dump(plt.gcf(), f)
    plt.close()

    # Re-sort the edge matrix acording to the node matrix variables
    Yedge = Yedge_aux[:,:,edge_ordering]  # Sort the columns
    Yedge = Yedge[:,edge_ordering,:]  # Sort the rows

    # Plot the sorted edge admittance for verification at the lowest frequency
    Yedge_nan = np.zeros_like(Yedge[0,:,:], dtype=np.float64)
    Yedge_nan.fill(np.nan)
    plt.imshow(20*np.log10(np.abs(Yedge[0, :, :]), out=Yedge_nan, where=(np.abs(Yedge[0,:,:])!=0.0)), cmap='spring', interpolation='nearest')
    plt.colorbar()
    plt.xticks(ticks=np.arange(0, len(node_variables), step=1), labels=[])
    plt.yticks(ticks=np.arange(0, len(node_variables), step=1), labels=node_variables)
    plt.grid(visible=True, which='minor', alpha=0.3, color='k', linestyle='-', linewidth=0.5)
    plt.title('Edge admittance matrix at '+format(frequencies[0], '.1f')+' Hz')
    plt.savefig(results_folder + '\\' + file_root + "_Edge.pdf", format="pdf", bbox_inches="tight")
    # with open(results_folder + '\\' + file_root + "_Edge.pickle", 'wb') as f: pickle.dump(plt.gcf(), f)
    plt.close()

    # Node admittance matrix and associated edge matrix
    Ynode = np.zeros((len(frequencies), len(node_variables), len(node_variables)),dtype='cdouble')  # Or dtype='csingle'
    y_node_idx = 0
    for idx, ynode in enumerate(node_matrix):
        # Rotate the matrix accordingly to the terminal angle if it involves AC variables
        if len(ynode.vars) >= 2:
            for block in block_area_angle:
                # Iterate over the different blocks and use the angle of the current matrix
                if block[0] in [bus[:-2] for bus in ynode.blocks]:
                    # The reference angle for the area of this block - bus angle of this block
                    theta = (reference_angle[block[1]] - block[2])
                    # print("Angle of",round(theta,4),"in the admittance involving",ynode.blocks)

        # Define the rotation matrix for each component: AC-side 2x2, AC/DC 3x3
        if ynode.y_type == "AC" and len(ynode.vars) == 2:
            T = np.array([[np.cos(theta), -np.sin(theta)],[np.sin(theta), np.cos(theta)]])
            T_1 = np.array([[np.cos(theta), np.sin(theta)], [-np.sin(theta), np.cos(theta)]])
        elif ynode.y_type == "ACDC" and len(ynode.vars) == 3:
            T = np.array([[1, 0, 0],[0, np.cos(theta), -np.sin(theta)],[0, np.sin(theta), np.cos(theta)]])
            T_1 = np.array([[1, 0, 0], [0, np.cos(theta), np.sin(theta)], [0, -np.sin(theta), np.cos(theta)]])
        else:
            T = np.identity(len(ynode.vas))  # DC-side analysis only: does not need frame alignment
            T_1 = np.identity(len(ynode.vars))

        # print('Matrix',idx,", nodal matrix start idx:",y_node_idx,"and end:",y_node_idx+len(ynode.vars))
        Ynode[:, y_node_idx:y_node_idx+len(ynode.vars), y_node_idx:y_node_idx+len(ynode.vars)] = T@ynode.y@T_1
        # Equivalent to np.matmul(T,np.matmul(ynode.y,T_1))
        y_node_idx = y_node_idx + len(ynode.vars)  # Update the matrix index for the next admittance block

    # Scatter verification of the node admittance for the lowest frequency
    Ynode_nan = np.zeros_like(Ynode[0,:,:], dtype=np.float64)
    Ynode_nan.fill(np.nan)
    plt.imshow(20*np.log10(np.abs(Ynode[0,:,:]), out=Ynode_nan, where=(np.abs(Ynode[0,:,:])!=0.0)), cmap='spring', interpolation='nearest')
    plt.colorbar()
    plt.xticks(ticks=np.arange(0, len(node_variables), step=1), labels=[])
    plt.yticks(ticks=np.arange(0, len(node_variables), step=1), labels=node_variables)
    plt.grid(visible=True, which='minor', alpha=0.3, color='k', linestyle='-', linewidth=0.5)
    plt.title('Node admittance matrix at '+format(frequencies[0], '.1f')+' Hz')
    plt.savefig(results_folder + '\\' + file_root + "_Node.pdf", format="pdf", bbox_inches="tight")
    # with open(results_folder + '\\' + file_root + "_Node.pickle", 'wb') as f: pickle.dump(plt.gcf(), f)
    plt.close()
    np.seterr(divide='warn')

    # Perform stability analysis
    L = np.matmul(np.linalg.inv(Yedge),Ynode)  # Loop gain matrix
    # Stability via eigenvalue loci
    stable = nyquist(L=L,frequencies=frequencies,results_folder=results_folder,filename=file_root,verbose=True,check_conditioning=check_conditioning,
                     condition_number_th=condition_number_th, make_plot=make_plot, indentations=indentations, save_pickle=save_pickle)
    # Stability via determinant
    nyquist_det(L=L,frequencies=frequencies,results_folder=results_folder,filename=file_root, verbose=False, offset=0.0,
                draw_arrows=True, show_plot=False, make_plot=make_plot, indentations=indentations, save_pickle=save_pickle) 

    # Oscillatory frequencies and bus participation factors based on the closed-loop impedance: the admittance is provided to avoid the inversion so Z_closedloop=False
    EVD(Yedge+Ynode, frequencies, node_variables, results_folder, file_root, make_plot=make_plot, Z_closedloop=False, save_pickle=save_pickle)

    # Save the admittance matrices
    results = [Yedge[:, row, col] for row in range(len(node_variables)) for col in range(len(node_variables))]
    results.insert(0, frequencies)
    results = tuple(results)
    np.savetxt(results_folder + '\\' + file_root + '_Y_edge.txt', np.stack(results, axis=1), delimiter='\t',
               header="f\t" + "\t".join(node_variables), comments='')

    results = [Ynode[:, row, col] for row in range(len(node_variables)) for col in range(len(node_variables))]
    results.insert(0, frequencies)
    results = tuple(results)
    np.savetxt(results_folder + '\\' + file_root + '_Y_node.txt', np.stack(results, axis=1), delimiter='\t',
               header="f\t" + "\t".join(node_variables), comments='')

    # Save the minor loop gain matrix
    results = [L[:, row, col] for row in range(len(node_variables)) for col in range(len(node_variables))]
    results.insert(0, frequencies)
    # elements = [str(row) + "-" + str(col) for row in range(len(node_variables)) for col in range(len(node_variables))]
    results = tuple(results)
    np.savetxt(results_folder+'\\'+file_root+'_Minor_loop_gain.txt',np.stack(results, axis=1),delimiter='\t',
               header="f\t" + "\t".join(node_variables), comments='')

    # Compute the passivity and SVD of the system matrices
    # passivity(G=Yedge_aux, frequencies=frequencies, results_folder=results_folder, filename=file_root + "_Yedge_aux",variables=edge_aux_variables)
    passivity(G=Ynode,frequencies=frequencies,results_folder=results_folder,filename=file_root+"_Ynode", variables=node_variables, make_plot=make_plot, save_pickle=save_pickle)
    passivity(G=Yedge, frequencies=frequencies, results_folder=results_folder, filename=file_root + "_Yedge", make_plot=make_plot, save_pickle=save_pickle)
    passivity(G=Ynode+Yedge, frequencies=frequencies, results_folder=results_folder, filename=file_root+"_Ynode_+_Yedge", make_plot=make_plot, save_pickle=save_pickle)
    small_gain(np.linalg.inv(Yedge), Ynode, frequencies, results_folder, file_root, variables=node_variables, make_plot=make_plot, save_pickle=save_pickle)

    return stable

def passivity(G, frequencies, results_folder=None, filename=None, variables=None, Yedge=None, make_plot=True, save_pickle=False):
    # The passivity index is computed as half of the minimum eigenvalue of the matrix plus its conjugate transpose
    # min{eig(A + A')}/2
    # A passive system has its Nyquist plot in the RHP: increased stability margins if connected to a passive system
    passivity_index = np.real(np.min(np.linalg.eig(G + G.swapaxes(-1, -2).conj())[0], axis=1))/2  # min{eig(A + A')}/2
    # The eigenvalues of a Hermitian matrix are always real but floating-point arithmetic renders a small complex part
    # and thus we take the real value at the end to get rid of the spurious numerical artifact.

    if (results_folder is not None) and (filename is not None):
        if not path.exists(results_folder): makedirs(results_folder)  # Create results folder if it does not exist
        
        # Plot the passivity index over the frequency range
        if variables is None and make_plot:
            fig, ax = plt.subplots(nrows=1, ncols=1, figsize=(8, 6))
            ax.plot(frequencies, passivity_index, color='blue', linestyle='solid', linewidth=2.0,label=r"$\mathbf{Y}_{node}$")
            ax.set_xscale("log")
            ax.minorticks_on()
            ax.grid(visible=True, which='major', color='k', linestyle='-', linewidth=0.5)
            # ax.grid(visible=True, which='minor', color='tab:gray', alpha=0.5, linestyle='-', linewidth=0.5)
            ax.set_ylabel(r'min $\{ \lambda (\mathbf{G} + \mathbf{G}^H) \}/2$')
            ax.set_title('Passivity evaluation for ' + str(len(frequencies)) + ' frequencies')
            ax.set_xlim([frequencies[0], frequencies[-1]])
            ax.set_xlabel('Frequency [Hz]')
            if Yedge is not None:
                passivity_index_Yedge = np.real(np.min(np.linalg.eig(Yedge+Yedge.swapaxes(-1, -2).conj())[0], axis=1))/2
                ax.plot(frequencies, passivity_index_Yedge, color='red', linestyle='solid', linewidth=2.0,label=r"$\mathbf{Y}_{edge}$")
                ax.plot(frequencies, passivity_index + passivity_index_Yedge, color='green', linestyle='dashed', linewidth=2.0,label=r'min $\{ \lambda (\mathbf{Y}_{node} + \mathbf{Y}_{node}^H) \}/2$+min $\{ \lambda (\mathbf{Y}_{edge} + \mathbf{Y}_{edge}^H) \}/2$')
                ax.plot(frequencies, np.real(np.min(np.linalg.eig(G + G.swapaxes(-1, -2).conj() + Yedge + Yedge.swapaxes(-1, -2).conj())[0],axis=1))/2, color='black', linestyle='dotted',linewidth=2.0, label=r"$\mathbf{Y}_{node}+\mathbf{Y}_{edge}$")

                ax.legend(loc='upper left', fancybox=True, shadow=True, ncol=2)
        elif make_plot:
            # Find the position of the block diagonal matrices as they are surrounded by zeros
            indices = []  # Tuple of start and end indeces of each matrix
            start_index = 0
            for index in range(G.shape[1]-1):
                if G[0, index, start_index] == 0:  # If the next element is zero, then boundary of block matrix if defined
                    indices.append((start_index, index-1))  # There is a block matrix between start_index and index
                    start_index = index  # Initialize start index of next matrix
            indices.append((start_index, G.shape[1] - 1))  # Last block matrix

            # Plot the passivity of the different matrices and the whole matrix
            fig, ax = plt.subplots(nrows=2, ncols=1, figsize=(8, 6))
            for block_pos in range(len(indices)):
                start_idx = indices[block_pos][0]
                end_idx = indices[block_pos][1] + 1
                A = G[:, start_idx:end_idx, start_idx:end_idx]
                passivity_index_block = np.real(np.min(np.linalg.eig(A + A.swapaxes(-1, -2).conj())[0], axis=1))/2
                ax[0].plot(frequencies, passivity_index_block, linestyle='solid', linewidth=2.0,label=", ".join(variables[start_idx:end_idx]))

            ax[0].set_xscale("log")
            ax[0].minorticks_on()
            ax[0].grid(visible=True, which='major', color='k', linestyle='-', linewidth=0.5)
            # ax[0].grid(visible=False)
            # ax[0].grid(visible=True, which='minor', color='tab:gray', alpha=0.5, linestyle='-', linewidth=0.5)
            ax[0].set_ylabel(r'min $\{ \lambda (\mathbf{G}_i + \mathbf{G}_i^H) \}/2$')
            ax[0].set_title('Passivity analysis for ' + str(len(frequencies)) + ' frequencies')
            ax[0].set_xlim([frequencies[0], frequencies[-1]])
            ax[0].legend(loc='upper right', ncol=int(np.floor(np.sqrt(len(indices)))), prop={'size': 6}) # fancybox=True, shadow=True, 

            ax[1].plot(frequencies,passivity_index,color='blue',linestyle='solid',linewidth=2.0,label="Complete matrix")
            ax[1].set_xscale("log")
            ax[1].minorticks_on()
            ax[1].grid(visible=True, which='major', color='k', linestyle='-', linewidth=0.5)
            # ax[1].grid(visible=True, which='minor', color='tab:gray', alpha=0.5, linestyle='-', linewidth=0.5)
            ax[0].set_ylabel(r'min $\{ \lambda (\mathbf{G} + \mathbf{G}^H) \}/2$')
            ax[1].legend(loc='upper left', fancybox=True, shadow=True, ncol=1)
            ax[1].set_xlim([frequencies[0], frequencies[-1]])
            ax[1].set_xlabel('Frequency [Hz]')
        
        if make_plot:
            fig.savefig(results_folder+'\\'+filename + "_passivity.pdf", format="pdf", bbox_inches="tight")
            if save_pickle:
                with open(results_folder + '\\' + filename + "_passivity.pickle", 'wb') as f: pickle.dump(fig, f)
            plt.close(fig)
        
        np.savetxt(results_folder+'\\'+filename+'_passivity.txt', np.stack((frequencies, passivity_index), axis=1), delimiter='\t',
                   header="f\t" + "Passivity_index", comments='')  

    return passivity_index

def nyquist(L, frequencies, results_folder=None, filename=None, verbose=True, check_conditioning=False, condition_number_th=0.01/5e-9, make_plot=True, show_plot=False, indentations =[], save_pickle=False):
    # Stability assessment via the Generalized Nyquist Criteria (GNC): graphycally determine the number of unstable closed-loop poles from the encirclements of the critical point by the loci of the open-loop gain matrix
    if verbose: print("Performing Nyquist stability assessment based on the eigenvalues of L")
    if not path.exists(results_folder): makedirs(results_folder)  # Create results folder if it does not exist

    # 1) Compute the eigenvalues of the loop-gain at every frequency
    eigenvalues = np.linalg.eig(L)[0]
    # Compute the condition number to discard doubtful data: threshold based on input error
    if check_conditioning: condition_number = np.linalg.cond(L)  # Relative error output <= cond_num * relative error input

    # Sorting of eigenvalues for a continuous eigenloci in the Nyquist plot based on minimum changes between frequencies
    eigenvalues_sorted = np.empty(eigenvalues.shape, dtype='cdouble')  # Initialization of sorted eigenvalues
    eigenvalues_sorted[0,:] = eigenvalues[0,:]  # First frequency as reference for the sorting
    for idx in range(1,eigenvalues.shape[0]):
        if check_conditioning:
            if condition_number[idx] > condition_number_th:
                eigenvalues_sorted[idx, :] = eigenvalues_sorted[idx-1, :] # Replace by previous well-conditioned values
        else:
            eig_1 = eigenvalues_sorted[idx - 1, :]  # Previous eigenvalues
            eig_2 = eigenvalues[idx, :]  # Current eigenvalues
            # Create matrix of distances between eigenvalues
            x, y = np.meshgrid(np.real(eig_1), np.real(eig_2), indexing='ij')
            d_real = np.abs(x - y)
            x, y = np.meshgrid(np.imag(eig_1), np.imag(eig_2), indexing='ij')
            d_imag = np.abs(x - y)
            d_abs = np.sqrt(np.square(d_real) + np.square(d_imag))  # Absolute distance by element-wise operations
            # Solve the linear sum assignment problem to find the minimum variation and thus the correct order
            col_ind = linear_sum_assignment(d_abs)[1]  # The absolute distance is the cost matrix
            eigenvalues_sorted[idx, :] = eigenvalues[idx, col_ind]  # Sort the eigenvalues

    # 2) Eigenloci plot and count clockwise and counter-clockwise encirclements of (-1,0j) for each eigenvalue
    # Compute the coordinates of the eigenloci for the GNC aplication and plotting
    x = np.real(eigenvalues_sorted)  # Real axis
    y = np.imag(eigenvalues_sorted)  # Imaginary axis
    
    # Consider only indentations strictly in the frequency range
    valid_indent = (indentations > frequencies[0]) & (indentations < frequencies[-1])
    inds = np.asarray(indentations)[valid_indent]

    j = np.searchsorted(frequencies, inds, side='left') # Candidate neighbor frequencies
    is_match = frequencies[j] == inds
    # Indices to the left of or at the indentations, and to the right of the indentations:
    # - If match:       (match_idx, match_idx + 1)
    # - Else (no match): (j-1, j)
    left_idx  = np.where(is_match, j,     j - 1)
    right_idx = np.where(is_match, j + 1, j)
    idx_pairs = np.stack([left_idx, right_idx], axis=1)
    idx_indentations = np.ravel(idx_pairs).tolist() # Save the two frequency indexes closest to each indentation frequency

    to_drop = np.unique(np.ravel(idx_pairs)) if idx_pairs.size else np.array([], dtype=int)
    keep_idx = np.ones(frequencies.size, dtype=bool)
    if to_drop.size: keep_idx[to_drop] = False # Boolean array to keep the points excluding those around the indentations
    
    x_indent = x.copy() # Make the values around the indentation NaN for plotting purposes
    y_indent = y.copy()
    x_indent[~keep_idx] = np.nan 
    y_indent[~keep_idx] = np.nan
    
    if len(indentations)>0 and verbose:
        for idx in range(0,len(idx_indentations),2):
            print(f"GNC indentation at {indentations[idx//2]} Hz performed between {frequencies[idx_indentations[idx]]} and {frequencies[idx_indentations[idx+1]]} Hz")
    
    cw = []  # List of clockwise encirclements for each locus
    ccw = []  # List of counter-clockwise encirclements for each locus
    if make_plot:
        fig, ax = plt.subplots(nrows=2, ncols=1, figsize=(6, 7))  # Create the figure and get the colors cycle
        colors = plt.rcParams['axes.prop_cycle'].by_key()['color']
        for col in plt.rcParams['axes.prop_cycle'].by_key()['color']: colors.append(col)  # Triplicate the color cycle
        for col in plt.rcParams['axes.prop_cycle'].by_key()['color']: colors.append(col)  # in case of many eigenvalues
    # Loop over the sorted eigenvalues for ploting the locus and count the encirclements
    for idx in range(eigenvalues_sorted.shape[1]):
        # Plot the eigenvalue locus avoiding the indentations
        if make_plot:
            ax[0].plot(x_indent[:,idx], y_indent[:,idx], color=colors[idx], linestyle='solid', linewidth=1.5, label=r'$\lambda_{'+format(idx+1,'.0f')+r'}$')
            ax[0].plot(x_indent[:,idx], -y_indent[:,idx], color=colors[idx], linestyle='solid', linewidth=1.5, label='_nolegend_')
            ax[1].plot(x_indent[:,idx],y_indent[:,idx], color=colors[idx],linestyle='solid',linewidth=1.5,label=r'$\lambda_{' + format(idx+1,'.0f')+r'}$')
            ax[1].plot(x_indent[:,idx], -y_indent[:,idx], color=colors[idx], linestyle='solid', linewidth=1.5, label='_nolegend_')

        # Count the number of (-1, 0j) encirclements by this eigenvalue locus
        cwi = 0  # Initialize the counters
        ccwi = 0
        for j in range(1,eigenvalues_sorted.shape[0]):
            # Only consider clockwise crossings of the imaginary axis beyond (-1,0j)
            if y[j - 1, idx] < 0 < y[j, idx] and (x[j,idx] < -1) and j not in idx_indentations:  # x[j-1,idx] < -1 or
                # Check that the (-1,0j) is to the right of the line between (x1,y1) and (x2,y2)
                # If the cross product of vectors (x2-x1, y2-y1) and (-1-x1, 0-y1) is < 0, then (-1,0) is to the right
                if (x[j,idx] - x[j-1,idx])*(0 - y[j-1,idx]) - (y[j,idx] - y[j-1,idx])*(-1 - x[j-1,idx]) < 0:
                    cwi += 1
                    if verbose: print("CW crossing around ",round(0.5*(frequencies[j] + frequencies[j-1]),4)," Hz by lambda =",str(idx+1))
                    if make_plot and show_plot:
                        fig1, ax1 = plt.subplots(nrows=1, ncols=1, figsize=(6, 7))
                        ax1.plot([x[j-1,idx],x[j,idx]],[y[j-1,idx],y[j,idx]], color='red', linestyle='solid', linewidth=2.0, label='_nolegend_')
                        ax1.scatter(x[j - 1, idx], y[j - 1, idx], color='green', label=str(frequencies[j-1]))
                        ax1.scatter(x[j, idx], y[j, idx], color='blue', label=str(frequencies[j]))
                        ax1.scatter(-1, 0, marker="+", c='black', label=r'$( -1, 0j )$')
                        ax1.legend(loc='upper right', ncol=1)
                        ax1.minorticks_on()
                        ax1.grid(visible=True, which='major', color='k', linestyle='-', linewidth=0.5)
                        if show_plot: plt.show()  # Visualize the plot 
                        with open(results_folder + '\\' + filename + "_GNC_lambda_"+str(idx)+"_cw_"+str(cwi)+".pickle", 'wb') as f: pickle.dump(fig1, f)
                        plt.close(fig1)


            # Only consider counter-clockwise crossings of the imaginary axis beyond (-1,0j)
            elif y[j - 1, idx] > 0 > y[j, idx] and (x[j-1,idx] < -1 or x[j,idx] < -1) and j not in idx_indentations:
                if (x[j,idx] - x[j-1,idx])*(0 - y[j-1,idx]) - (y[j,idx] - y[j-1,idx])*(-1 - x[j-1,idx]) > 0:
                    # If the cross product of vectors (x2-x1, y2-y1) and (-1-x1, 0-y1) is > 0, then (-1,0) is to the left
                    # if (x[j,idx] - x[j-1,idx])*(0 - y[j-1,idx]) - (y[j,idx] - y[j-1,idx])*(-1 - x[j-1,idx]) > 0:
                    ccwi += 1
                    if verbose: print("CCW crossing around ",round(0.5*(frequencies[j] + frequencies[j-1]),4)," Hz by lambda =",str(idx+1))
                    if make_plot and show_plot:
                        fig1, ax1 = plt.subplots(nrows=1, ncols=1, figsize=(6, 7))
                        ax1.plot([x[j-1,idx],x[j,idx]],[y[j-1,idx],y[j,idx]], color='red', linestyle='solid', linewidth=2.0, label='_nolegend_')
                        ax1.scatter(x[j - 1, idx], y[j - 1, idx], color='green', label=str(frequencies[j-1]))
                        ax1.scatter(x[j, idx], y[j, idx], color='blue', label=str(frequencies[j]))
                        ax1.scatter(-1, 0, marker="+", c='black', label=r'$( -1, 0j )$')
                        ax1.legend(loc='upper right', ncol=1)
                        ax1.minorticks_on()
                        ax1.grid(visible=True, which='major', color='k', linestyle='-', linewidth=0.5)
                        if show_plot: plt.show()  # Visualize the plot
                        with open(results_folder + '\\' + filename + "_GNC_lambda_"+str(idx)+"_ccw_"+str(ccwi)+".pickle", 'wb') as f: pickle.dump(fig1, f)
                        plt.close(fig1)

        cw.append(cwi)  # Add the counters to the list
        ccw.append(ccwi)
    # print("CC: ",cw,"\nCCW: ",ccw)
    N = sum(cw) - sum(ccw)  # Net number of clockwise encirclements
    if N > 0:
        stable_system = False
        if verbose: print("\n GNC stability assessment: unstable closed-loop system \n")
    elif N < 0:
        stable_system = False
        if verbose: print("\n GNC stability assessment: unstable subsystem \n")
    else:
        stable_system = True
        if verbose: print("\n GNC stability assessment: stable closed-loop system if subsystems are stable \n")

    # Plot the unit circle and the critical point
    if make_plot:
        th = np.linspace(-np.pi * 1.01, np.pi * 1.01, 314)
        ax[0].plot(np.cos(th), np.sin(th), color='black', linestyle='dotted', linewidth=1.0, label='Unit circle')
        ax[0].scatter(-1, 0, marker="+", c='blue', label=r'$( -1, 0j )$')
        ax[0].minorticks_on()
        ax[0].grid(visible=True, which='major', color='k', linestyle='-', linewidth=0.5)
        # ax[0].grid(visible=True, which='minor', color='tab:gray', alpha=0.5, linestyle='-', linewidth=0.5)
        ax[0].set_title('Eigenloci between '+format(frequencies[0],'.1f')+' and '+format(frequencies[-1],'.1f') + ' Hz')
        ax[0].set_xlim([np.min(x,axis=None), np.max(x,axis=None)])
        ax[0].set_ylim([np.min(np.concatenate((-y,y)),axis=None), np.max(np.concatenate((-y,y)),axis=None)])
        ax[0].set_xlabel('Real axis')
        ax[0].set_ylabel('Imaginary axis')
        ax[0].legend(loc='upper right', ncol=4, prop={'size': 6})

        ax[1].plot(np.cos(th), np.sin(th), color='black', linestyle='dotted', linewidth=1.0, label='Unit circle')
        ax[1].scatter(-1, 0,s=4*rcParams['lines.markersize'] ** 2, marker="+", c='blue', label=r'$( -1, 0j )$')
        ax[1].set_xlim([-2.0, 2.0])
        ax[1].set_ylim([-2.0, 2.0])
        ax[1].set_xlabel('Real axis')
        ax[1].set_ylabel('Imaginary axis')
        ax[1].minorticks_on()
        ax[1].grid(visible=False) # which='major', color='k', linestyle='-', linewidth=0.5
        fig.savefig(results_folder + '\\' + filename + "_GNC.pdf", format="pdf", bbox_inches="tight")
        if save_pickle:
            with open(results_folder + '\\' + filename + "_GNC.pickle", 'wb') as f: pickle.dump(fig, f)
        if show_plot: plt.show()  # Visualize the plot
        plt.close(fig)

    # Save the eigenloci
    loci = [eigenvalues_sorted[:, idx] for idx in range(eigenvalues_sorted.shape[1])]
    loci.insert(0, frequencies)
    loci = tuple(loci)
    np.savetxt(results_folder + '\\' + filename + '_GNC.txt', np.stack(loci, axis=1), delimiter='\t',
               header="f\t"+"\t".join(["lambda_"+format(idx+1,'.0f') for idx in range(eigenvalues_sorted.shape[1])]), comments='')

    return stable_system

def small_gain(G1, G2, frequencies, results_folder=None, filename=None, variables=None, make_plot=True, save_pickle=False):
    # Applies a conservative version of the small-gain theorem as |L| = |G1*G2| <= |G1|*|G2| < 1

    S1 = np.linalg.svd(G1, compute_uv=False)
    S2 = np.linalg.svd(G2, compute_uv=False)
    S12 = np.linalg.svd(np.matmul(G1,G2), compute_uv=False)

    if not path.exists(results_folder): makedirs(results_folder)  # Create results folder if it does not exist

    if make_plot:
        fig, ax = plt.subplots(nrows=2, ncols=1, figsize=(8, 6))
        ax[0].plot(frequencies, 1.0/np.max(S1, axis=1), color='blue', linestyle='solid', linewidth=2.0, label=r"1 / max $\sigma (\mathbf{G}_1)$")
        if variables is not None:
            # Find the position of the block diagonal matrices as they are surrounded by zeros
            indices = []  # Tuple of start and end indeces of each matrix
            start_index = 0
            for index in range(G2.shape[1] - 1):
                if G2[0, index, start_index] == 0:  # If the next element is zero, then boundary of block matrix if defined
                    indices.append((start_index, index - 1))  # There is a block matrix between start_index and index
                    start_index = index  # Initialize start index of next matrix
            indices.append((start_index, G2.shape[1] - 1))  # Last block matrix

            # Plot the maximum singular values of the different block matrices and the whole matrix
            for block_pos in range(len(indices)):
                start_idx = indices[block_pos][0]
                end_idx = indices[block_pos][1] + 1
                S2_block = np.max(np.linalg.svd(G2[:,start_idx:end_idx,start_idx:end_idx], compute_uv=False), axis=1)
                ax[0].plot(frequencies, S2_block, linestyle='solid',linewidth=2.0, label=", ".join(variables[start_idx:end_idx]))
        ax[0].plot(frequencies, np.max(S2, axis=1), color='red', linestyle='dashed', linewidth=2.0,label=r"max $\sigma (\mathbf{G}_2)$")

        # Setings for upper plot
        ax[0].set_xscale("log")
        ax[0].set_yscale("log")
        ax[0].minorticks_on()
        ax[0].grid(visible=True, which='major', color='k', linestyle='-', linewidth=0.5)
        # ax[0].grid(visible=True, which='minor', color='tab:gray', alpha=0.5, linestyle='-', linewidth=0.5)
        ax[0].set_ylabel(r'max $\sigma ( \cdot )$')
        ax[0].set_title('Singular value analysis over ' + str(len(frequencies)) + ' frequencies')
        ax[0].set_xlim([frequencies[0], frequencies[-1]])
        # ax[0].set_xlabel('Frequency [Hz]')
        ax[0].legend(loc='best', fancybox=True, shadow=True, ncol=1, prop={'size': 6})

        ax[1].plot(frequencies, np.max(S12, axis=1), color='black', linestyle='solid', linewidth=2.0, label=r"max $\sigma (\mathbf{G}_1  \mathbf{G}_2)$")
        ax[1].plot(frequencies, np.multiply(np.max(S1, axis=1), np.max(S2, axis=1)), color='green', linestyle='dashed',
                linewidth=2.0, label=r"max $\sigma (\mathbf{G}_1) \cdot $ max $\sigma (\mathbf{G}_2)$")
        ax[1].plot([frequencies[0], frequencies[-1]],[1, 1], color='grey', linestyle='dotted',linewidth=2.0, label='_nolegend_')
        ax[1].set_xscale("log")
        ax[1].set_yscale("log")
        ax[1].minorticks_on()
        ax[1].grid(visible=True, which='major', color='k', linestyle='-', linewidth=0.5)
        # ax[1].grid(visible=True, which='minor', color='tab:gray', alpha=0.5, linestyle='-', linewidth=0.5)
        ax[1].set_ylabel('Unitless')
        ax[1].set_xlim([frequencies[0], frequencies[-1]])
        ax[1].set_xlabel('Frequency [Hz]')
        ax[1].legend(loc='best', fancybox=True, shadow=True, ncol=1, prop={'size': 6})
        
        fig.savefig(results_folder + '\\' + filename + "_gain.pdf", format="pdf", bbox_inches="tight")
        if save_pickle:
            with open(results_folder + '\\' + filename + "_gain.pickle", 'wb') as f: pickle.dump(fig, f)
        plt.close(fig)

    np.savetxt(results_folder + '\\' + filename + '_gain.txt',
                np.stack((frequencies, np.max(S1, axis=1), np.max(S2, axis=1), np.max(S12, axis=1)), axis=1),
                delimiter='\t', header="f\t" + "max_sigma_G1\t" + "max_sigma_G2\t" + "max_sigma_G1_G2", comments='')

def EVD(G, frequencies, bus_names=None, results_folder=None, filename=None, verbose=True, Z_closedloop=True, make_plot=True, save_pickle=False):
    if bus_names is None: bus_names = [str(bus+1) for bus in range(G.shape[1])]  # Sorted numbers if names not provided
    if not path.exists(results_folder): makedirs(results_folder)  # Create results folder if it does not exist

    # 1) Eigenvalue decomposition over the frequency
    eigenvalues, right_eigenvectors = np.linalg.eig(G)

    if not Z_closedloop: eigenvalues = 1.0 / eigenvalues  # Eigenvalues of G^-1 are the inverse of the eigenvalues of G
    
    # Sorting of eigenvalues for a continuous plot based on minimum changes between adjacent frequencies
    eigenvalues_sorted = np.empty(eigenvalues.shape, dtype='cdouble')  # Initialization of sorted eigenvalues
    eigenvalues_sorted[0, :] = eigenvalues[0, :]  # First frequency as reference for the sorting
    for idx in range(1, eigenvalues.shape[0]):
        eig_1 = eigenvalues_sorted[idx - 1, :]  # Previous eigenvalues
        eig_2 = eigenvalues[idx, :]  # Current eigenvalues
        # Create matrix of distances between eigenvalues
        x, y = np.meshgrid(np.real(eig_1), np.real(eig_2), indexing='ij')
        d_real = np.abs(x - y)
        x, y = np.meshgrid(np.imag(eig_1), np.imag(eig_2), indexing='ij')
        d_imag = np.abs(x - y)
        d_abs = np.sqrt(np.square(d_real) + np.square(d_imag))  # Absolute distance by element-wise operations
        # Solve the linear sum assignment problem to find the minimum variation and thus the correct order
        col_ind = linear_sum_assignment(d_abs)[1]  # The absolute distance is the cost matrix
        eigenvalues_sorted[idx, :] = eigenvalues[idx, col_ind]  # Sort the eigenvalues
        right_eigenvectors[idx, :] = right_eigenvectors[idx][:,col_ind]  # Sort the eigenvectors

    left_eigenvectors = np.linalg.inv(right_eigenvectors)

    # 2) Oscillation modes identification
    lambda_re = np.real(eigenvalues_sorted)  # Real part
    lambda_imag = np.real(eigenvalues_sorted)  # Imaginary part
    lambda_abs = np.abs(eigenvalues_sorted)  # Absolute value (magnitude)

    # 2.1) Oscillation mode identification based on the magnitude peaks of G = closed-loop impedance matrix
    idx_lambda_max = np.argmax(lambda_abs,axis=0)  # Frequency index of the maximum magnitude of each eigenvalue
    idx_lambda_max_max = np.argmax([lambda_abs[idx_lambda_max[idx],idx] for idx in range(eigenvalues.shape[1])])  # Critical mode = the highest mag peak
    freq_idx = idx_lambda_max[idx_lambda_max_max]  # Oscillation frequency index; or also freq_indices[idx_lambda_min]
    if verbose: print("The main oscillation frequency is around ",round(frequencies[freq_idx],2),"Hz based on the magnitude of eigenvalue",idx_lambda_max_max+1,"=",np.round(eigenvalues_sorted[idx_lambda_max[idx_lambda_max_max],idx_lambda_max_max], 5))

    # # 2.2) Based on the minimum real part at imaginary zero-crossing (used in the Positive Net Damping criterion)
    # sign_changes = np.diff(np.sign(lambda_imag),axis=0)
    # critical_points = np.nonzero(sign_changes)  # Frequency of sign change from + to - or viceversa of the imaginary parts
    # freq_indices = critical_points[0]
    # lambda_indices = critical_points[1]
    # # Frequency index of the minimum real part of each eigenvalue over the critical frequencies
    # critical_lambdas = [lambda_re[freq_indices[idx], lambda_indices[idx]] for idx in range(len(freq_indices))]  # Real part at the critical frequencies
    # idx_lambda_min = np.argmin(critical_lambdas)
    # print("According to the PND, the critical mode is at",round(frequencies[freq_indices[idx_lambda_min]],3),"Hz based on the minimum real part of eigenvalue",lambda_indices[idx_lambda_min]+1)
    # print(" Eigenvalue",lambda_indices[idx_lambda_min]+1, "=",round(eigenvalues_sorted[freq_indices[idx_lambda_min],lambda_indices[idx_lambda_min]],5))
    # print(" The slope of the imaginary part is",round(lambda_imag[freq_indices[idx_lambda_min],lambda_indices[idx_lambda_min]]-lambda_imag[freq_indices[idx_lambda_min]-1,lambda_indices[idx_lambda_min]],5))
    # freq_idx = freq_indices[idx_lambda_min] # Oscillation frequency index or also round(critical_lambdas[idx_lambda_min],5)

    # 3) Compute the bus participation factors (PFs) of the critical eigenvalue at the oscillation frequency
    # Controllability (right eigenvectors) and observability (transpose of left eigenvectors)
    Obs = right_eigenvectors[freq_idx, :]
    Cont = np.transpose(left_eigenvectors[freq_idx, :])
    # PF[frequency, row = bus, column = mode]
    PF = right_eigenvectors * left_eigenvectors.transpose(0,2,1)  # Element-wise product of right eigenvectors and the transposed left eigenvectors
    PF_freq_idx = Obs * Cont  # Element-wise product
    PF_mode = PF_freq_idx[:,idx_lambda_max_max]  # Select the target mode

    # The controllability, observability and PF of the critical mode at each bus
    if verbose: print("Bus"+(max([len(bus) for bus in bus_names])-3)*" "+"\t","Cont.\t","Obs.\t","PF")  # Header
    for idx, bus in enumerate(bus_names):
        if verbose: 
            print(bus+"\t", f"{np.abs(Cont[idx,idx_lambda_max_max]) / np.sum(np.abs(Cont[:,idx_lambda_max_max])):.4f}"+"\t",
              f"{np.abs(Obs[idx,idx_lambda_max_max]) / np.sum(np.abs(Obs[:,idx_lambda_max_max])):.4f}"+"\t",
              f"{np.abs(PF_mode[idx]) / np.sum(np.abs(PF_mode)):.4f}")

    # 4) Plot the eigenvalues over frequency
    if make_plot:
        fig, ax = plt.subplots(nrows=3, ncols=1, figsize=(6, 8))  # Create the figure and get the colors cycle
        colors = plt.rcParams['axes.prop_cycle'].by_key()['color']
        for col in plt.rcParams['axes.prop_cycle'].by_key()['color']: colors.append(col)  # Triplicate colour cycle
        for col in plt.rcParams['axes.prop_cycle'].by_key()['color']: colors.append(col)  # in case of many eigenvalues
        # Loop over the sorted eigenvalues and plot them over the frequency range
        for idx in range(eigenvalues_sorted.shape[1]):
            # Plot the eigenvalue locus
            ax[0].plot(frequencies, lambda_abs[:,idx], color=colors[idx], linestyle='solid', linewidth=1.5,
                    label=r'$\lambda_{' + format(idx + 1, '.0f') + r'}$')
            ax[1].plot(frequencies, lambda_re[:, idx], color=colors[idx], linestyle='solid', linewidth=1.5,
                    label=r'$\lambda_{' + format(idx + 1, '.0f') + r'}$')
            ax[2].plot(frequencies, lambda_imag[:, idx], color=colors[idx], linestyle='solid', linewidth=1.5,
                    label=r'$\lambda_{' + format(idx + 1, '.0f') + r'}$')

        # Figure settings and save to pdf
        ax[0].minorticks_on()
        ax[0].grid(visible=True, which='major', color='k', linestyle='-', linewidth=0.5)
        # ax[0].grid(visible=True, which='minor', color='tab:gray', alpha=0.5, linestyle='-', linewidth=0.5)
        ax[0].set_title('Eigenvalue decomposition between ' + format(frequencies[0], '.1f') + ' and ' + format(frequencies[-1], '.1f') + ' Hz')
        ax[0].set_xlim([frequencies[0], frequencies[-1]])
        ax[0].set_ylim([np.min(lambda_abs, axis=None), np.max(lambda_abs, axis=None)])
        ax[0].set_ylabel('Magnitude')
        ax[0].set_xscale("log")
        ax[0].set_yscale("log")
        ax[0].legend(loc='lower right', ncol=int(np.ceil(np.sqrt(eigenvalues.shape[1]))), prop={'size': 6})

        ax[1].minorticks_on()
        ax[1].grid(visible=True, which='major', color='k', linestyle='-', linewidth=0.5)
        # ax[1].grid(visible=True, which='minor', color='tab:gray', alpha=0.5, linestyle='-', linewidth=0.5)
        ax[1].set_xlim([frequencies[0], frequencies[-1]])
        ax[1].set_ylim([np.min(lambda_re, axis=None), np.max(lambda_re, axis=None)])
        ax[1].set_ylabel('Real part')
        ax[1].set_xscale("log")

        ax[2].minorticks_on()
        ax[2].grid(visible=True, which='major', color='k', linestyle='-', linewidth=0.5)
        # ax[2].grid(visible=True, which='minor', color='tab:gray', alpha=0.5, linestyle='-', linewidth=0.5)
        ax[2].set_xlim([frequencies[0], frequencies[-1]])
        ax[2].set_ylim([np.min(lambda_imag, axis=None), np.max(lambda_imag, axis=None)])
        ax[2].set_xlabel('Frequency [Hz]')
        ax[2].set_ylabel('Imaginary part')
        ax[2].set_xscale("log")

        # plt.show()  # Visualize the plot interactively
        fig.savefig(results_folder + '\\' + filename + "_EVD.pdf", format="pdf", bbox_inches="tight")
        
        if save_pickle:
            with open(results_folder + '\\' + filename + "_EVD.pickle", 'wb') as f: pickle.dump(plt.gcf(), f)
        plt.close(fig)

    # Save the EVD into a text file
    evd_results = [eigenvalues_sorted[:,idx] for idx in range(eigenvalues_sorted.shape[1])]
    evd_results.insert(0, frequencies)
    evd_results = tuple(evd_results)
    np.savetxt(results_folder + '\\' + filename + '_EVD.txt', np.stack(evd_results, axis=1), delimiter='\t',
               header="Frequency [Hz]\t" + "\t".join(bus_names), comments='')

def nyquist_det(L, frequencies, results_folder=None, filename=None, verbose=True, offset=0.0, draw_arrows=True, make_plot=True, show_plot=False, f0=50.0, indentations=[], save_pickle=False):
    # Stability assessment based on the determinant of I + L
    if verbose: print("Performing Nyquist stability assessment based on the determinant of I + L")
    if not path.exists(results_folder): makedirs(results_folder)  # Create results folder if it does not exist
    
    # Compute the determinant
    det = np.linalg.det(np.identity(L.shape[1]) + L) + offset  
    x = np.real(det)
    y = np.imag(det)

    # Consider only indentations strictly in the frequency range
    valid_indent = (indentations > frequencies[0]) & (indentations < frequencies[-1])
    inds = np.asarray(indentations)[valid_indent]
    
    j = np.searchsorted(frequencies, inds, side='left') # Candidate neighbor frequencies
    is_match = frequencies[j] == inds
    # Indices to the left of or at the indentations, and to the right of the indentations:
    # - If match:       (match_idx, match_idx + 1)
    # - Else (no match): (j-1, j)
    left_idx  = np.where(is_match, j,     j - 1)
    right_idx = np.where(is_match, j + 1, j)

    idx_pairs = np.stack([left_idx, right_idx], axis=1)
    idx_indentations = np.ravel(idx_pairs).tolist() # Save the two frequency indexes closest to each indentation frequency

    to_drop = np.unique(np.ravel(idx_pairs)) if idx_pairs.size else np.array([], dtype=int)
    keep_idx = np.ones(frequencies.size, dtype=bool)
    if to_drop.size: keep_idx[to_drop] = False # Boolean array to keep the points excluding those around the indentations
    
    x_indent = x.copy()
    y_indent = y.copy()
    x_indent[~keep_idx] = np.nan # Make the values around the indentation NaN to avoid plotting lines across them
    y_indent[~keep_idx] = np.nan
    
    if len(indentations)>0 and verbose:
        for idx in range(0,len(idx_indentations),2):
            print(f"GNC indentation at {indentations[idx//2]} Hz performed between {frequencies[idx_indentations[idx]]} and {frequencies[idx_indentations[idx+1]]} Hz")

    # Count the number of (offset, 0j) encirclements by det
    cwi = 0  # Initialize the counters
    ccwi = 0
    for j in range(1,len(frequencies)):
        # Clockwise crossings of the positive vertical axis at (offset,0j) avoiding the indentations
        if x[j - 1] < offset < x[j] and j not in idx_indentations:
            # Check that the (offset,0j) is to the right of the line between (x1,y1) and (x2,y2)
            # If the cross product of vectors (x2-x1, y2-y1) and (offset-x1, 0-y1) is < 0, then (offset,0) is to the right
            if (x[j] - x[j-1])*(0 - y[j-1]) - (y[j] - y[j-1])*(offset - x[j-1]) < 0:
                cwi += 1
                if show_plot:
                    print("CW crossing around ",round(0.5*(frequencies[j] + frequencies[j-1]),4)," Hz")
                    fig1, ax1 = plt.subplots(nrows=1, ncols=1, figsize=(6, 7))
                    ax1.plot([x[j-1],x[j]],[y[j-1],y[j]], color='red', linestyle='solid', linewidth=2.0, label='_nolegend_')
                    ax1.scatter(x[j-1], y[j-1], color='green', label=str(frequencies[j-1]))
                    ax1.scatter(x[j], y[j], color='blue', label=str(frequencies[j]))
                    ax1.scatter(offset, 0, marker="+", c='black', label=r'$( -1, 0j )$')
                    ax1.legend(loc='upper right', ncol=1)
                    ax1.minorticks_on()
                    ax1.grid(visible=True, which='major', color='k', linestyle='-', linewidth=0.5)
                    with open(results_folder + '\\' + filename + "_det_cw_"+str(cwi)+".pickle", 'wb') as f: pickle.dump(fig1, f)
                    if show_plot: plt.show() 
                    plt.close(fig1)

        # Counter-clockwise crossings of the positive vertical axis at (offset,0j) avoiding the indentations
        elif x[j - 1] > offset > x[j] and j not in idx_indentations:
            if (x[j] - x[j-1])*(0 - y[j-1]) - (y[j] - y[j-1])*(offset - x[j-1]) > 0:
                # If the cross product of vectors (x2-x1, y2-y1) and (offset-x1, 0-y1) is > 0, then (offset,0) is to the left
                ccwi += 1
                if show_plot:
                    print("CCW crossing around ",round(0.5*(frequencies[j] + frequencies[j-1]),4)," Hz")
                    fig1, ax1 = plt.subplots(nrows=1, ncols=1, figsize=(6, 7))
                    ax1.plot([x[j-1],x[j]],[y[j-1],y[j]], color='red', linestyle='solid', linewidth=2.0, label='_nolegend_')
                    ax1.scatter(x[j-1], y[j-1], color='green', label=str(frequencies[j-1]))
                    ax1.scatter(x[j], y[j], color='blue', label=str(frequencies[j]))
                    ax1.scatter(offset, 0, marker="+", c='black', label=r'$( -1, 0j )$')
                    ax1.legend(loc='upper right', ncol=1)
                    ax1.minorticks_on()
                    ax1.grid(visible=True, which='major', color='k', linestyle='-', linewidth=0.5)
                    with open(results_folder + '\\' + filename + "_det_ccw_"+str(ccwi)+".pickle", 'wb') as f: pickle.dump(fig1, f)
                    if show_plot: plt.show() 
                    plt.close(fig1)

    N = cwi - ccwi  # Net number of clockwise encirclements
    if N > 0:
        stable_system = False
        if verbose: print("\n GNC stability assessment: unstable closed-loop system \n")
    elif N < 0:
        stable_system = False
        if verbose: print("\n GNC stability assessment: unstable subsystem \n")
    else:
        stable_system = True
        if verbose: print("\n GNC stability assessment: stable closed-loop system if subsystems are stable \n")

    if make_plot:
        fig, ax = plt.subplots(nrows=2, ncols=1, figsize=(6, 7))  # Create the figure
        ax[0].scatter(offset, 0, marker="+", c='blue', label=r'$( '+str(round(offset,0))+', 0j )$') # Plot the critical point
        ax[0].plot(x_indent, y_indent, color='red', linestyle='solid', linewidth=1.5)
        if draw_arrows:
            da = int(np.log(x_indent.shape[0] + 1))  # decimate the number of arrows
            ax[0].quiver(x_indent[::da], y_indent[::da], np.gradient(x_indent)[::da], np.gradient(y_indent)[::da], angles='xy', scale_units='xy', scale=da, linewidth=1, edgecolor='black', facecolor='green')
        id0 = np.argmin(np.abs(frequencies - f0))
        ax[0].text(x[id0], y[id0], str(round(frequencies[id0], 2)) + ' Hz', fontsize=12, color='blue', ha='right', va='bottom')
        ax[0].text(x[-1], y[-1], str(round(frequencies[-1], 2)) + ' Hz', fontsize=12, color='blue', ha='right', va='bottom')
        ax[0].text(x[0], y[0], str(round(frequencies[0], 2)) + ' Hz', fontsize=12, color='blue', ha='right', va='bottom')

        ax[0].minorticks_on()
        ax[0].grid(visible=True, which='major', color='k', alpha=0.5, linestyle='-', linewidth=0.5)
        ax[0].set_title(str(offset)+r' + det[I + L(s)] between '+format(frequencies[0], '.1f')+' and ' + format(frequencies[-1], '.1f') + ' Hz')
        ax[0].set_xlim([np.min(x, axis=None), np.max(x, axis=None)])
        ax[0].set_ylim([np.min(y, axis=None), np.max(y, axis=None)])
        ax[0].set_xlabel('Real axis')
        ax[0].set_ylabel('Imaginary axis')
        ax[0].legend(loc='best', ncol=1)

        ax[1].plot(x_indent, y_indent, color='red', linestyle='solid', linewidth=2.0)
        # if draw_arrows:
        #     ax[1].quiver(x_indent, y_indent, np.gradient(x_indent), np.gradient(y_indent), angles='xy', scale_units='xy', scale=1, linewidth=1, edgecolor='black', facecolor='green')
        ax[1].scatter(offset, 0, s=4 * rcParams['lines.markersize'] ** 2, marker="+", c='blue', label=r'$( ' + str(offset) + ', 0j )$')
        id0 = np.argmin(np.abs(x + y*1j - offset)) # Closest point to the critical point
        ax[1].text(x[id0], y[id0], str(round(frequencies[id0], 2)) + ' Hz', fontsize=12, color='blue', ha='right', va='bottom')
        ax[1].set_xlim([-1.0-1.2*np.abs(x[id0]), 1.0+1.2*np.abs(x[id0])])
        ax[1].set_ylim([-1.0-1.2*np.abs(y[id0]), 1.0+1.2*np.abs(y[id0])])
        ax[1].minorticks_on()
        ax[1].grid(visible=True, which='major', color='k', alpha=0.5, linestyle='-', linewidth=0.5)
        ax[1].set_xlabel('Real axis')
        ax[1].set_ylabel('Imaginary axis')

        fig.savefig(results_folder + '\\' + filename + "_det.pdf", format="pdf", bbox_inches="tight")
        if save_pickle:
            with open(results_folder + '\\' + filename + "_det.pickle", 'wb') as f: pickle.dump(fig, f)
        if show_plot: plt.show()  # Visualize the plot interactively
        plt.close(fig)

    # Save the results
    np.savetxt(results_folder + '\\' + filename + '_det.txt', np.stack((frequencies,det), axis=-1), delimiter='\t',
               header="Frequency [Hz]\t"+str(offset)+"+det[I + L(s)]", comments='')
    
    return stable_system

# def stability_margins(L, frequencies, results_folder=None, filename=None, critical_point=-1, type="P", verbose=True, make_plot=True, save_pickle=False):
#     # Stability margins based on the minimum distance of the eigenloci to the critical point, the phase crossover frequency or the gain crossover frequency
#     if verbose: print("Computing stability margins with respect to the critical point ("+str(critical_point) + ", 0j)")
#     if not path.exists(results_folder): makedirs(results_folder)  # Create results folder if it does not exist

#     if type == "P":
#         # Phase margin: phase of the open-loop loci at the gain crossover frequency (where the magnitude is 1)
#         mag = np.abs(L)  # Vector magnitude
#         gain_crossing = np.where(np.diff(np.sign(mag - 1.0)))[0]

#     elif type == "G":
#         # Gain margin: minimum distance to the critical point (0,0j)
#         critical_point = 0
#     elif type == "V":
#         print("The stability margin type 'V' is not implemented yet. Please choose 'P' for phase margin or 'G' for gain margin.")
#     else:
#         print(" Invalid margin type. Please choose 'type=P' for phase margin, 'type=G' for gain margin or 'type=V' for vector margin.")
    

#     # Compute the distance to the critical point (critical_point, 0j)
#     distance = np.abs(L - critical_point)  # Distance to the critical point by element-wise operations

#     # Minimum distance over frequency
#     min_distance = np.min(distance, axis=0)

#     if make_plot:
#         fig, ax = plt.subplots(nrows=1, ncols=1, figsize=(6, 7))  # Create the figure
#         ax.plot(frequencies, min_distance, color='red', linestyle='solid', linewidth=2.0)
#         ax.set_xscale("log")
#         ax.set_yscale("log")
#         ax.minorticks_on()
#         ax.grid(visible=True, which='major', color='k', linestyle='-', linewidth=0.5)
#         ax.set_title('Minimum distance of eigenloci to (' + str(critical_point) + ',0j) between ' + format(frequencies[0], '.1f') + ' and ' + format(frequencies[-1], '.1f') + ' Hz')
#         ax.set_xlim([frequencies[0], frequencies[-1]])
#         ax.set_ylim([np.min(min_distance), np.max(min_distance)])
#         ax.set_xlabel('Frequency [Hz]')
#         ax.set_ylabel('Minimum distance to (' + str(critical_point) + ',0j)')
#         fig.savefig(results_folder + '\\' + filename + "_margins.pdf", format="pdf", bbox_inches="tight")
#         if save_pickle:
#             with open(results_folder + '\\' + filename + "_margins.pickle", 'wb') as f: pickle.dump(fig, f)
#         plt.close(fig)

#     # Save the results
#     np.savetxt(results_folder + '\\' + filename + '_margins.txt', np.stack((frequencies,min_distance), axis=-1), delimiter='\t',
#                header="Frequency [Hz]\tMinimum distance to (-1,0j)")

#     return margin

nyquist.__doc__ = """
Stability assessment based the Generalized Nyquist Criteria (GNC): eigenvalue decomponsition (EVD) of the open-loop (minor-loop) matrix over the frequency.

The GNC can be stated as follows considering a contour along the imaginary axis and around the whole Right-Half Plane (RHP) avoiding open-loop poles:
N: Net number of clockwise encirclements by the open-loop eigenloci =  clockwise -  counter-clockwise
P: Number of RHP poles of the open-loop system
Z: number of RHP poles of the closed-loop system
Argument principle states N = Z - P over the closed contour. Therefore, if Z = N + P > 0, then the closed-loop system has RHP poles and it is unstable.
Assuming standalone-stable subsystems means P = 0, and thus N = 0 implies stability while N > 0 implies instability.
If the subsystems are standalone unstable, P > 0, then possibly N < 0.
The interested reader is referred to S. Skogestad and I. Postlethwaite, "Multivariable Feedback Control: Analysis and Design", Wiley, 2005 for a more detailed explanation.

The function computes the eigenvalues of L at every frequency, plots the eigenloci and counts the number of clockwise and counter-clockwise encirclements of (-1,0j).
The EVD of L is saved as filename_GNC.txt and its plot is saved as filename_GNC.pdf.

Required arguments
        L                   (numpy ndarray of complex double) Minor loop gain (transfer matrix) for different frequencies.
        frequencies         (numpy array) Frequencies over which L is computed [Hz].
        results_folder      (str) Absolute path where the results are to be stored. If it does not exist, it is created.
        filename            (str) Name root of the results output files.        
      
Optional arguments
        verbose             (bool) Bool flag to show detailed GNC application information, such as the number of counter clock-wise (CCW) and clock-wise (CC) encirclements of the critical point.
        check_conditioning  (bool) Bool flag to discard values with poor numerical conditioning of L.
        indentations        (list of double) Frequencies [Hz] at which indentations around open-loop poles are performed.
        condition_number_th (double) Condition number threshold of L above which the data is ignored.
                            This threshold can be set based on the expected input error and maximum acceptable ouTput error.
                            For example, for a relative output error <= 0.01 considering a relative input error of 5e-9, the condition number threshold can be set to 0.01/5e-9 (default value).
        save_pickle         (bool) Bool flag to save the generated plots as pickle objects in addition to pdf files. Default = False.

Returns
        Bool flag indicating closed-loop or interconnected stability: True means stable.

"""

nyquist_det.__doc__ = """
Stability assessment based on based the Generalized Nyquist Criteria (GNC) by counting the encirclements of the critical point by the determinant of I + L over the frequency.

Theorem 4.14 in S. Skogestad and I. Postlethwaite, "Multivariable Feedback Control: Analysis and Design", Wiley (2005), assuming standalone stable subsystems,
i.e. L(s) does not have any open-loop unstable poles, the stability conditions are:
a) Zero net number of clockwise encirclements of (0,j0) by det[I+L(s)] as s travels the imaginary axis from 0 to +j*infinity avoiding the pure imaginary poles of L
b) No crossings of the orgin by by det[I + L(s)] as s travels the imaginary axis from 0 to +j*infinity
Since only real systems are considered in practice, it implies that det[I + L(+j*infinity)] settles on the real axis and just a large enough frequency can be used to approximate det[I + L(+j*infinity)].

The interested reader is referred to S. Skogestad and I. Postlethwaite, "Multivariable Feedback Control: Analysis and Design", Wiley, 2005 for a more detailed explanation.

The function computes the eigenvalues of L at every frequency, plots the eigenloci and counts the number of clockwise and counter-clockwise encirclements of (-1,0j).
The EVD of L is saved as filename_GNC.txt and its plot is saved as filename_GNC.pdf.

Required arguments
        L                   (numpy ndarray of complex double) Minor loop gain (transfer matrix) for different frequencies.
        frequencies         (numpy array) Frequencies over which L is computed [Hz].
        results_folder      (str) Absolute path where the results are to be stored. If it does not exist, it is created.
        filename            (str) Name root of the results output files.        
      
Optional arguments
        indentations        (list of double) Frequencies [Hz] at which indentations around open-loop poles are performed.
        verbose             Bool flag to show detailed GNC application information, such as the number of counter clock-wise (CCW) and clock-wise (CC) encirclements of the critical point. Default = True.
        draw_arrows         Bool flag to draw arrows on the direction of the Nyquist plot. Default = True.
        offset              (double) The offset parameter can be used to shift the critical point on the real axis instead of (0,j0). Default = 0.
                            For example, offset = -1.0 defines the critical point as (-1,j0) as when applying the GNC via the eigenvalue loci of L.
        show_plot           (bool) to show the plot interactively. Default = False.
        save_pickle         Bool flag to save the generated plots as pickle objects in addition to pdf files. Default = False.

Returns
        Bool flag indicating closed-loop or interconnected stability: True means stable.

"""

stability_analysis.__doc__ = """
Performs a small-signal analysis based on the scanned matrices of the network defined by the topology file over the frequency and stores the results in the specified results folder.
Firstly, the function reads the individual admittance matrices from the specified topology file. Then it builds the edge and node (block-diagonal) matrices, including the necessary rotations to a common reference frame.
Finally, it applies different dynamic evaluations to assess the small-signal dynamics of the system:
- Generalized Nyquist Criteria (GNC) based on the eigenvalue decomposition (EVD) and the determinant of the open-loop (minor-loop) matrix L computed as the product of the edge and node matrices.
- Eigenvalue decomposition (EVD) of the closed-loop impedance matrix for oscillation modes identification.
- Bus participation factors computation at the frequency of largest current amplification.
- Small-gain theorem based on the singular value decomposition (SVD) of the edge and node matrices:: used to identify risk-free frequencies.
- Passivity assessment based on the minimum singular value of the Hermitian part of the different system matrix: used to identify risk-free frequencies.

Required arguments
        topology            (str) Absolute path to the topology file defining the system the subsystems interconnections.
        results_folder      (str) Absolute path where the matrices and powerflow files are stored.
        file_root           (str) Name root of the files used in the analysis
Optional arguments
        check_conditioning  (bool) Bool flag to discard values with poor numerical conditioning of the system matrices.
        condition_number_th (double) Condition number threshold of the system matrices above which the data is ignored.
                            This threshold can be set based on the expected input error and maximum acceptable ouTput error.
        make_plot           (bool) Bool flag to enable/disable the generation of pdf plot files.
        indentations        (list of double) Frequencies [Hz] at which indentations around open-loop poles are performed in the GNC.
        save_pickle         (bool) Bool flag to save the generated plots as pickle objects in addition to pdf files. Default = False.
"""

passivity.__doc__ = """
Passivity assessment based on the minimum eigenvalue value of the Hermitian part of the system matrices over the frequency.
If the system under evaluation is stable with a positive passivity index across all frequencies, then no instability can arise when interconnected to another passive system.
Required arguments
        G                   (numpy ndarray of complex double) System matrix at different frequencies.
        frequencies         (numpy array) Frequencies over which the matrix G is evaluated [Hz].
        results_folder      (str)Absolute path where the results are to be stored.
        filename            (str) Name root of the results output files.
        
Optional arguments
        variables           (list of str) Names of the block-diagonal matrices in G for block-wise analysis. Default = None.
        make_plot           (bool) Bool flag to enable/disable the generation of pdf plot files.
        save_pickle         (bool) Bool flag to save the generated plots as pickle objects in addition to pdf files. Default = False.
"""

EVD.__doc__ = """
Eigenvalue decomposition (EVD) of the closed-loop system matrix over the frequency for oscillation modes identification and bus participation factors (PF) computation.
The function is based on the developements presented in https://doi.org/10.1109/TPWRD.2004.834856 and https://doi.org/10.1016/j.ijepes.2023.108957
Required arguments
        G                   (numpy ndarray of complex double) Closed-loop system matrix at different frequencies.
        frequencies         (numpy array) Frequencies over which the matrix G is evaluated [Hz].
        results_folder      (str) Absolute path where the results are to be stored.
        filename            (str) Name root of the results output files.
Optional arguments
        bus_names          (list of str) Names of the buses in the system for PF computation. Default = None.
        verbose            (bool) Bool flag to show detailed analysis information.
        Z_closedloop       (bool) Bool flag indicating if G is the closed-loop impedance matrix. This can be used to avoid the inversion of Ynode + Yedge. Default = True.
        make_plot          (bool) Bool flag to enable/disable the generation of pdf plot files.
        save_pickle        (bool) Bool flag to save the generated plots as pickle objects in addition to pdf files. Default = False.
"""

small_gain.__doc__ = """
Applies a conservative version of the small-gain theorem verifying if |L| = |G1*G2| <= |G1|*|G2| < 1 holds by plotting
the maximum singular value of G1, G2 and G1*G2 over the frequency as well as the unitary gain line.
If the unitary gain line is not crossed by the maximum singular value of G1*G2, then no instability can arise at said frequency.
To visually check this, the plot of 1/|G1| is compared with that of |G2|, since if |G2| < 1/|G1|, then |G1*G2| < 1.
Therefore, the Bode plot of |G2| should be below 1/|G1| to guarantee stability.

Required arguments
        G1                  (numpy ndarray of complex double) System matrix at different frequencies.
        G2                  (numpy ndarray of complex double) System matrix at different frequencies.
        frequencies         (numpy array) Frequencies over which the matrix G is evaluated [Hz].
        results_folder      (str) Absolute path where the results are to be stored.
        filename            (str) Name root of the results output files.
 
Optional arguments
        variables           (list of str) Names of the block-diagonal matrices in G2 for block-wise analysis. Default = None.
        make_plot           (bool) Bool flag to enable/disable the generation of pdf plot files.
        save_pickle         (bool) Bool flag to save the generated plots as pickle objects in addition to pdf files. Default = False.
"""

"""
Stability margins based on the minimum distance of the eigenvalues of the closed-loop system matrix to the imaginary axis over the frequency.
The function takes the eigenvalues of G at every frequency, checks the minimum distance to the imaginary axis and reports the results.

Required arguments
        G                   (numpy ndarray of complex double) eigenvalues of the open-loop matrix or vector at different frequencies.
        frequencies         (numpy array) Frequencies over which G is evaluated [Hz].
        results_folder      (str) Absolute path where the results are to be stored.
        filename            (str) Name root of the results output files.  
        type                (str) Type of margin to compute: "P" for phase margin, "G" for gain margin, and "V" for vector margin. Default = "P".

"""