# -*- coding: utf-8 -*-
"""
Copyright or © or Copr. Project-Team S3 (Sound Signals and Systems) and Analysis/Synthesis team,
Laboratory of Sciences and Technologies of Music and Sound (UMR 9912),
IRCAM-CNRS-UPMC, 1 place Igor Stravinsky, F-75004 Paris
contributor(s) : Antoine Falaize, Thomas Hélie, Thu Jul 9 23:11:37 2015
corresponding contributor: antoine.falaize@ircam.fr

This software (pypHs) is a computer program whose purpose is to generate C++ 
code for the simulation of multiphysics system described in graph formalism. 
It is composed of a library (pypHs.py) and a dictionnary (Dictionnary.py)

This software is governed by the CeCILL-B license under French law and
abiding by the rules of distribution of free software.  You can  use, 
modify and/ or redistribute the software under the terms of the CeCILL-B
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info". 

As a counterpart to the access to the source code and  rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty  and the software's author,  the holder of the
economic rights,  and the successive licensors  have only  limited
liability. 

In this respect, the user's attention is drawn to the risks associated
with loading,  using,  modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean  that it is complicated to manipulate,  and  that  also
therefore means  that it is reserved for developers  and  experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or 
data to be ensured and,  more generally, to use and operate it in the 
same conditions as regards security. 

The fact that you are presently reading this means that you have had
knowledge of the CeCILL-B license and that you accept its terms.

"""

from multiprocessing import Process, Queue
import numpy as np        
import sympy as sp
import scipy as sc
import networkx as nx
import progressbar as pb
import matplotlib.pyplot as plt
from sympy.printing import ccode
from sympy.physics import units
import os
import time

eps = np.finfo(float).eps

def SymbolicGaussJordanInverse(OriginalMatrix,  SimplifyIn=True, SimplifyOut=True):
    ##############################################################################
    nl,nc = OriginalMatrix.shape
    SymbMatrixDummified = sp.zeros(nl,nc)    
    bar = ProgressBar(nl+nl+nl*nc,10,'GaussJordan Inverse')
    bar.update(0)
    for n in np.arange(nl):
        for m in np.arange(nc):
            symb = sp.symbols('s'+str(n)+str(m))
            SymbMatrixDummified[n,m] = symb
        bar.update(n+1)

    SymbMatrix = sp.Matrix.hstack(sp.Matrix.copy(SymbMatrixDummified),sp.eye(nl))  
    ##############################################################################
    for n in np.arange(nl):
        nn = n
        while SymbMatrix[nn:n] == 0:
            nn += 1
        SymbMatrix[n,:], SymbMatrix[nn,:] = SymbMatrix[nn,:], SymbMatrix[n,:]
        Mnn = SymbMatrix[n,n]
        SymbMatrix[n,:] = SymbMatrix[n,:]/Mnn
        a = list(np.arange(nl))
        a.remove(n)
        for mm in a:
            Mtemp = sp.Matrix.copy(SymbMatrix[mm,:] - SymbMatrix[mm,n]*SymbMatrix[n,:]) if SymbMatrix[mm,n] != 0 else sp.Matrix.copy(SymbMatrix[mm,:])
            SymbMatrix[mm,:] = Mtemp
        bar.update(nl+n+1)
    iSymbMatrixDummified = SymbMatrix[:,nl:]
    iSymbMatrixDummified = iSymbMatrixDummified.applyfunc(lambda x: sp.powsimp(x.factor())) if SimplifyOut else iSymbMatrixDummified
    ##############################################################################
    for n in np.arange(nl):
        for m in np.arange(nc):
            original = sp.powsimp(OriginalMatrix[n,m].factor()) if SimplifyIn else OriginalMatrix[n,m]
            iSymbMatrixDummified = iSymbMatrixDummified.subs(sp.symbols('s'+str(n)+str(m)),original)
            bar.update(nl+nl+n*nc+m+1)
    ##############################################################################
    return iSymbMatrixDummified
    
fontsize = 28
legendfontsize = 30
linewidth = 2.

figwidth = 10.
figheight_1 = 5.
figheight_2 = 10.
figheight_3 = 15.

left  = 0.17  # the left side of the subplots of the figure
right = 0.95    # the right side of the subplots of the figure
bottom = 0.15   # the bottom of the subplots of the figure
top = 0.9      # the top of the subplots of the figure
wspace = 0.2   # the amount of width reserved for blank space between subplots
hspace = 0.3  # the amount of height reserved for white space between subplots
axedef = [left , bottom, right, top, wspace, hspace]


class PortHamiltonianObject:
    """ Object oriented sympy (symbolic) representation of a 
        port-Hamiltonian (pH) System structured as follows: \n
        - storage: dictionary with entries x, xl, xnl, H, Q, Hnl, realizability\n
        - dissipation: dictionary with entries w, wl, wnl, z, R, znl, realizability\n
        - ports: dictionary with entries u, y, realizability\n
        - Graph: NetworkX MultidiGraph structure with branches xl, xnl, zl, znl, p\n
    """

    def __init__(self, label=None, folder=None):
        self.label = label
        self.folder = os.getcwd() if folder is None else folder
        self.H = sp.sympify(0)
        self.x = []
        self.w = []
        self.z = []
        self.u = []
        self.y = []
        self.params = []
        self.diagQ = []
        self.diagR = []
        self.diagG = []
        self.Graph = nx.MultiDiGraph()
        self.ConstantInputs = []
        self.ConstantParameters = []
        self.transformers = []
        self.trans_u = []
        self.trans_y = []
        self.unities = {'x':[], 'w':[], 'u':[]}
        self.subs = []
        self.subs_par = []

        self.Fl = []
        self.Fnl = []
        self.dxH = []
        self.gradH = []
        self.PDJ = sp.sympify(0)
        
        self.ch_of_var = []
        
##############################################################################
##############################################################################
    
    def nx(self):
        return self.x.__len__()
    def nw(self):
        return self.w.__len__()
    def ny(self):
        return self.y.__len__()
    def np(self):
        return self.params.__len__()
    def ntot(self):
        return self.nx()+self.nw()+self.ny()
    def ntrans(self):
        return self.trans_y.__len__()

    def all_vars(self):
        return self.x+self.w+self.y

    def nxl(self):
        return self.diagQ.__len__()
    def nxnl(self):
        return self.nx() - self.nxl()

    def nwl(self):
        return self.diagR.__len__()
    def nwnl(self):
        return self.nw() - self.nwl()

    def xl(self):
        return self.x[:self.nxl()]
    def xnl(self):
        return self.x[self.nxl():]
    def dxl(self):
        return self.dx[:self.nxl()]
    def dxnl(self):
        return self.dx[self.nxl():]

    def wl(self):
        return self.w[:self.nwl()]
    def wnl(self):
        return self.w[self.nwl():]

    def zl(self):
        return self.z[:self.nwl()]
    def znl(self):
        return self.z[self.nwl():]

    def Q(self):
        return sp.diag(*np.array(self.diagQ))
    def R(self):
        return sp.diag(*np.array(self.diagR))

    def units_x(self):
        return self.unities['x']
    def units_dtx(self):
        return [e/units.s for e in self.unities['x']]
    def units_dxH(self):
        return [units.power*units.s/e for e in self.unities['x']]

    def units_w(self):
        return self.unities['w']
    def units_z(self):
        return [units.power/e for e in self.unities['w']]

    def units_u(self):
        return self.unities['u']
    def units_y(self):
        return [units.power/e for e in self.unities['u']]

    def Gains(self):
        return sp.diag(*np.array(self.diagG))

##############################################################################
    
    
    def stateChange(self, f):
#        self.old_vars_from_new_vars = 
        x_tilde = sp.symbols(['c'+str(x) for x in self.x])
        g = sp.solve([el_f-el_x for (el_f, el_x) in zip(f, x_tilde)], self.x, dict=True, exclude=self.params)
        if type(g) is list:
            if g.__len__()>1:
                print("Inverse mapping is not unique. Choose one of:")
                n=0
                for el_g in g:
                    n += 1
                    print("\n")
                    print(str(n)+":")
                    print(el_g)            
                n = int(raw_input("Choice?\n"))
                g = g[n-1]
            else:
                g = g[0]
        G = dict(g)
        g = []
        for xx in zip(self.x,x_tilde):
            if any([xx[0]==k for k in G.keys()]):
                g += [G[xx[0]]]
            else:
                g += [xx[1]]
        M = sp.eye(self.ntot())
        for l in range(self.nx()):
            for c in range(self.nx()):
                M[l,c] = f[l].diff(self.x[c]).doit()
        for sub in zip(self.x,g):
            M = M.subs(sub[0], sub[1])
        self.x_old_from_new = g
        self.J = M*self.J*M.T
        for n in range(self.nx()):
            self.applySubs([(self.x[n],g[n])])
        self.x = x_tilde
        for n in range(self.nxl()):
            self.diagQ[n] = self.H.diff(self.x[n],2)    
        
    def linearVariableChange(self, index, gain):
        if index < self.nx():
            n = index
            f = [x for x in self.x]
            f[n] = f[n]/gain
            self.stateChange(f)
            self.applySubs()
            
        elif (index>=self.nx()) &  (index<self.nx()+self.nw()):
            n = index-self.nx()
            old_symb = self.w[n]
            new_symb = sp.symbols('c'+str(old_symb))
            old_as_func_of_new = gain*new_symb
            self.z[n] = gain*self.z[n].subs(old_symb, old_as_func_of_new)
            self.w[n] = new_symb
            if n<self.nwl():
                self.diagR[n] = self.z[n].diff(self.w[n]).doit()
            M = sp.eye(self.ntot())
            M[index,index] = gain**-1
            self.applySubs([(old_symb,old_as_func_of_new)])
            self.J = M*self.J*M.T

        elif (index>=self.nx()+self.nw()) &  (index<self.ntot()):
            n = index-self.nx()-self.nw()
            old_symb = self.u[n]
            M = sp.eye(self.ntot())
            M[index,index] = gain**-1
            self.J = M*self.J*M.T
            self.diagG[n] = self.diagG[n]*gain
            self.applySubs()

    def Canonize(self, nonlinearize=False):

        # for each line of J
        list_ind = range(self.nx()+self.nw())
        list_ind.reverse()
        for l in list_ind:
            # analyse element c to find nonzeros
            indices_nonzeros = []
            for c in range(self.ntot()):
                element_of_J = self.J[l,c]
                if element_of_J != sp.sympify(0):
                    indices_nonzeros.append(c)    
            # for each nonzero element
            for c in indices_nonzeros:
                # if J[l,c] is not canonic
                if sp.Abs(self.J[l,c]) != sp.sympify(1) :
                    gain = self.J[l,c]
                    # get indices of connected edges
                    indices_connections = indices_nonzeros
                    indices_connections.remove(c)
                    # For each connected element
                    self.linearVariableChange(l, gain)
                    for i in indices_connections:
                        # replace with gain = 1/J[l,c]
                        if sp.Abs(self.J[l,i]) != sp.sympify(1) :
                            self.linearVariableChange(i, gain**-1)
                    # replace element l

        if nonlinearize:
            list_ind = range(self.nxl())
            list_ind.reverse()
            while list_ind:
                n = list_ind.pop()
                if any([ any( el == p for p in self.params) for el in list(sp.sympify(self.diagQ[n]).free_symbols)]):
                    self.moveStateLinToNLin(n)
                    list_ind = [el-1 for el in list_ind]
            list_ind = range(self.nwl())
            list_ind.reverse()
            while list_ind:
                n = list_ind.pop()
                test_par = any([ any( el == p for p in self.params) for el in list(sp.sympify(self.diagR[n]).free_symbols)])
                test_state = any([ any( el == x for x in self.x) for el in list(sp.sympify(self.diagR[n]).free_symbols)])
                if any([test_state,test_par]):
                    self.moveDissLinToNLin(n)
                    list_ind = [el-1 for el in list_ind]
                

##############################################################################

    def AddStructure(self, Jx, K, Gx, Jw, Gw, Jy):

        self.Jx = Jx if type(Jx) == sp.Matrix else sp.Matrix(Jx)
        self.K = K if type(K) == sp.Matrix else sp.Matrix(K)
        self.Gx = Gx if type(Gx) == sp.Matrix else sp.Matrix(Gx)
        self.Jw = Jw if type(Jw) == sp.Matrix else sp.Matrix(Jw)
        self.Gw = Gw if type(Gw) == sp.Matrix else sp.Matrix(Gw)
        self.Jy = Jy if type(Jy) == sp.Matrix else sp.Matrix(Jy)

        self.J = sp.Matrix.vstack(sp.Matrix.hstack(self.Jx,-self.K,self.Gx),sp.Matrix.hstack(self.K.T,self.Jw,self.Gw), sp.Matrix.hstack(-self.Gx.T,-self.Gw.T,self.Jy))

    def moveJLine(self, indi, indf):
        if indi < indf:
            deb = sp.Matrix.vstack(self.J[:indi,:], self.J[indi+1:indf+1,:])
            end = self.J[indf+1:,:]
            eli = self.J[indi,:]
            self.J =  sp.Matrix.vstack(deb, eli, end)
        elif indi > indf:
            deb = self.J[:indf,:]
            end = sp.Matrix.vstack(self.J[indf:indi,:], self.J[indi+1:,:])
            eli = self.J[indi,:]
            self.J =  sp.Matrix.vstack(deb, eli, end)
    
    def moveJCol(self, indi, indf):
        if indi < indf:
            deb = sp.Matrix.hstack(self.J[:,:indi], self.J[:,indi+1:indf+1])
            end = self.J[:,indf+1:]
            eli = self.J[:,indi]
            self.J =  sp.Matrix.hstack(deb, eli, end)
        elif indi > indf:
            deb = self.J[:,:indf]
            end = sp.Matrix.hstack(self.J[:,indf:indi], self.J[:,indi+1:])
            eli = self.J[:,indi]
            self.J =  sp.Matrix.hstack(deb, eli, end)
        
    def moveState(self, indi, indf):
        if indi < indf:
            deb = self.x[:indi] + self.x[indi+1:indf+1]
            end = self.x[indf+1:]
            eli = [self.x[indi]]
            self.x = deb + eli + end
            self.moveJCol(indi,indf)
            self.moveJLine(indi,indf)
            if indi<self.nxl():
                debQ = self.diagQ[:indi] + self.diagQ[indi+1:indf+1]
                endQ = self.diagQ[indf+1:]
                eliQ = [self.diagQ[indi]]
                self.diagQ = debQ + eliQ + endQ
                
        elif indi > indf:
            deb = self.x[:indf]
            end = self.x[indf:indi]+ self.x[indi+1:]
            eli = [self.x[indi]]
            self.x = deb + eli + end
            self.moveJCol(indi,indf)
            self.moveJLine(indi,indf)
            if indi<self.nxl():
                debQ = self.diagQ[:indf]
                endQ = self.diagQ[indf:indi]+ self.diagQ[indi+1:]
                eliQ = [self.diagQ[indi]]
                self.diagQ = debQ + eliQ + endQ

    def moveStateLinToNLin(self, indi):
        self.moveState(indi, self.nxl()-1)
        self.diagQ = self.diagQ[:-1]

    def moveDiss(self, indi, indf):
        if indi < indf:
            deb = self.w[:indi] + self.w[indi+1:indf+1]
            end = self.w[indf+1:]
            eli = [self.w[indi]]
            self.w = deb + eli + end
            deb = self.z[:indi] + self.z[indi+1:indf+1]
            end = self.z[indf+1:]
            eli = [self.z[indi]]
            self.z = deb + eli + end
            self.moveJCol(indi+self.nx(),indf+self.nx())
            self.moveJLine(indi+self.nx(),indf+self.nx())
            if indi<self.nwl():
                debR = self.diagR[:indi] + self.diagR[indi+1:indf+1]
                endR = self.diagR[indf+1:]
                eliR = [self.diagR[indi]]
                self.diagR = debR + eliR + endR
                
        elif indi > indf:
            deb = self.w[:indf]
            end = self.w[indf:indi]+ self.x[indi+1:]
            eli = [self.w[indi]]
            self.w = deb + eli + end
            deb = self.z[:indf]
            end = self.z[indf:indi]+ self.z[indi+1:]
            eli = [self.z[indi]]
            self.z = deb + eli + end
            self.moveJCol(indi+self.nx(),indf+self.nx())
            self.moveJLine(indi+self.nx(),indf+self.nx())
            if indi<self.nwl():
                debR = self.diagR[:indf]
                endR = self.diagR[indf:indi]+ self.diagR[indi+1:]
                eliR = [self.diagR[indi]]
                self.diagR = debR + eliR + endR

    def moveDissLinToNLin(self, indi):
        self.moveDiss(indi, self.nwl()-1)
        self.diagR = self.diagR[:-1]
        
                
        
        
#######################################################################

    def AddLinearStorageComponents(self,ListStringsXComp,ListValsQComp, ListUnits=[]):
        self.unities['x'] += ListUnits
        xComp = sp.symbols(ListStringsXComp)
        self.x = self.x[:self.nxl()] + xComp + self.x[self.nxl():] 
        self.diagQ = self.diagQ + ListValsQComp
        self.H = self.H + (sp.Matrix(xComp).T*sp.diag(*ListValsQComp)*sp.Matrix(xComp))[0,0]/2
        
    def AddNonLinearStorageComponents(self,ListSymbolsXComp,ExprHComp, ListUnits=[]):
        self.unities['x'] += ListUnits
        self.x = self.x + ListSymbolsXComp
        self.H = self.H + ExprHComp

    def AddLinearDissipativeComponents(self,ListStringsWComp,ListValsRComp, ListUnits=[]):
        self.unities['w'] += ListUnits
        wComp = sp.symbols(ListStringsWComp)
        self.w = self.w[:self.nwl()] + wComp + self.w[self.nwl():]
        self.z = self.z[:self.nwl()] + list(sp.diag(*ListValsRComp)*sp.Matrix(wComp)) + self.z[self.nwl():]  
        self.diagR = self.diagR+ListValsRComp

    def AddNonLinearDissipativeComponents(self,ListSymbolsWComp,ListExprZComp, ListUnits=[]):
        self.unities['w'] += ListUnits
        self.w = self.w + ListSymbolsWComp
        self.z = self.z + ListExprZComp

    def AddPorts(self,ListSymbolsU,ListSymbolsY, ListUnits=[]):
        self.unities['u'] += ListUnits
        self.u = self.u + ListSymbolsU
        self.y = self.y + ListSymbolsY
        self.diagG += [1]*ListSymbolsY.__len__()
#######################################################################

    def AddTransformers(self, label, transform_type, parameter):
        """
        AddTransformers(label:string, transform_type, parameter:str)
        Transform type is "gyrator" or "transformator"
        """
        u1, u2 = sp.symbols(['u_'+label+'_'+str(e+1) for e in range(2)])
        y1, y2 = sp.symbols(['y_'+label+'_'+str(e+1) for e in range(2)])
        alpha = sp.symbols(parameter) if type(parameter) == str else parameter
        transformer = {'u':(u1,u2), 'y':(y1,y2), 'alpha':alpha, 'type':transform_type}
        self.AddParameters([alpha])
        self.trans_u += [u1,u2]
        self.trans_y += [y1,y2]
        self.transformers += [transformer]

#######################################################################

    def AddParameters(self,ListSymbolsParams):
        self.params += ListSymbolsParams

    def AddConstantInpu(self, symbol, value):
        self.ConstantInputs += (symbol,value)

#######################################################################

    def Build(self, fs=sp.symbols('fs'), NumTol=eps, FixedPars=None, ExportCpp=True, replace=True, simplify=True, parallelize=True):

        
        
        ## Parallel factorization
        def put_factorization_in_queue(q,expr,n):
            q.put([expr.factor(),n])
            
        def parallel_factorize(expr_list):
            n_list = expr_list.__len__()
            q = Queue()
            proc = list()
            for i in range(n_list):
                proc.append(Process(target=put_factorization_in_queue, args=(q,expr_list[i],i)))
            for p in proc:
                p.start()                
            for p in proc:
                p.join()
            out = [0,]*n_list
            for i in range(n_list):
                el = q.get()
                out[el[1]] = el[0]
            return out
                
            
        if any([type(fs) is float, type(fs) is int]):
            self.fs=fs
        elif type(fs) is sp.Symbol:
            self.fs = sp.symbols('fs')
            self.params = [self.fs] + self.params

        if replace:
            self.replaceParByExpr()

        # Split Linear and Nonlinear part
        print("\nSplit")
        time.sleep(0.1)
        self.J1 = self.Jx[:self.nxl(),:self.nxl()]        
        self.M1 = self.Jx[:self.nxl(),self.nxl():]        
        self.J2 = self.Jx[self.nxl():,self.nxl():]        

        self.K1 = self.K[:self.nxl(),:self.nwl()]        
        self.K2 = self.K[:self.nxl(),self.nwl():]        
        self.K3 = self.K[self.nxl():,:self.nwl()]        
        self.K4 = self.K[self.nxl():,self.nwl():]        

        self.G1 = self.Gx[:self.nxl(),:]        
        self.G2 = self.Gx[self.nxl():,:]        

        self.J3 = self.Jw[:self.nwl(),:self.nwl()]        
        self.M2 = self.Jw[:self.nwl(),self.nwl():]        
        self.J4 = self.Jw[self.nwl():,self.nwl():]        

        self.G3 = self.Gw[:self.nwl(),:]        
        self.G4 = self.Gw[self.nwl():,:]        
                
        #######################################################################        

        print("Explicit dissipative inverse")
        time.sleep(0.1)
        self.iDw = sp.eye(self.nwl())- self.J3*self.R()        
        self.Dw = sp.Matrix.inv(self.iDw, method='LU')
#        self.Dw = SymbolicGaussJordanInverse(self.iDw, SimplifyIn=True, SimplifyOut=simplify)
        #######################################################################
        print("Resistive interconnection")
        time.sleep(.1)
        self.A1 = self.Dw*self.K1.T
        self.B1 = self.Dw*self.K3.T
        self.C1 = self.Dw*self.M2
        self.D1 = self.Dw*self.G3
        #######################################################################
        self.tildeA2 = self.J1-self.K1*self.R()*self.A1
        self.tildeB2 = self.M1-self.K1*self.R()*self.B1
        self.tildeC2 = -(self.K2+self.K1*self.R()*self.C1)
        self.tildeD2 = self.G1-self.K1*self.R()*self.D1
        #######################################################################
        print("Explicit storage inverse")
        time.sleep(.1)
        self.iDx = sp.eye(self.nxl())*self.fs - self.tildeA2*self.Q()/2.
#        self.Dx = SymbolicGaussJordanInverse(self.iDx.applyfunc(lambda x: sp.simplify(x)))
#        self.Dx = sp.Matrix.inv(self.iDx).applyfunc(lambda x: sp.simplify(x))
        self.Dx = sp.Matrix.inv(self.iDx)
#        self.Dx = SymbolicGaussJordanInverse(self.iDx, SimplifyIn=False, SimplifyOut=simplify)
        print("Dynamical interconnection")
        time.sleep(.1)
#        self.A2 = (self.Dx*self.tildeA2*self.Q()).applyfunc(lambda x: sp.simplify(x))
        self.A2 = self.Dx*self.tildeA2*self.Q()
        self.B2 = self.Dx*self.tildeB2
        self.C2 = self.Dx*self.tildeC2
        self.D2 = self.Dx*self.tildeD2
        #######################################################################
        self.A3 = self.Q()*(sp.eye(self.nxl())+(sp.sympify(1)/sp.sympify(2))*self.A2)
        self.B3 = (sp.sympify(1)/sp.sympify(2))*self.Q()*self.B2
        self.C3 = (sp.sympify(1)/sp.sympify(2))*self.Q()*self.C2
        self.D3 = (sp.sympify(1)/sp.sympify(2))*self.Q()*self.D2
        #######################################################################
        self.tildeA4 = -(self.M1.T+ self.K3*self.R()*self.A1)
        self.tildeB4 = (self.J2 - self.K3*self.R()*self.B1)
        self.tildeC4 = -(self.K4 + self.K3*self.R()*self.C1)
        self.tildeD4 = (self.G2 - self.K3*self.R()*self.D1)
        #######################################################################
        self.A4 = self.tildeA4*self.A3
        self.B4 = self.tildeB4+self.tildeA4*self.B3
        self.C4 = self.tildeC4+self.tildeA4*self.C3
        self.D4 = self.tildeD4+self.tildeA4*self.D3
        #######################################################################
        self.tildeA5 = (self.K2.T- self.M2.T*self.R()*self.A1)
        self.tildeB5 = (self.K4.T - self.M2.T*self.R()*self.B1)
        self.tildeC5 = (self.J4 - self.M2.T*self.R()*self.C1)
        self.tildeD5 = (self.G4 - self.M2.T*self.R()*self.D1)
        #######################################################################
        self.A5 = self.tildeA5*self.A3
        self.B5 = (self.tildeB5+self.tildeA5*self.B3)
        self.C5 = (self.tildeC5+self.tildeA5*self.C3)
        self.D5 = (self.tildeD5+self.tildeA5*self.D3)
        #######################################################################
        self.A6 = self.A1*self.A3
        self.B6 = (self.B1+self.A1*self.B3)
        self.C6 = (self.C1+self.A1*self.C3)
        self.D6 = (self.D1+self.A1*self.D3)
        #######################################################################
        print("Discrete Gradient")
        time.sleep(.1)
        self.dx = [sp.symbols('d'+str(xx)) for xx in self.x]
        #######################################################################

        self.dxHl = list(self.Q()*(sp.sympify(2)*sp.Matrix(self.xl()) + sp.Matrix(self.dxl()))/sp.sympify(2))

        self.dxHnl = []
        for n in range(self.nxnl()):
            dxh1 = sp.simplify(( self.H.subs(self.xnl()[n],self.xnl()[n]+self.dxnl()[n])-self.H)/self.dxnl()[n])
            dxh2 = sp.simplify(self.H.diff(self.xnl()[n])).doit()
            dxh = sp.Piecewise((dxh1,self.dxnl()[n]<-NumTol),(dxh2,self.dxnl()[n]<NumTol),(dxh1,True))
            self.dxHnl.append(dxh)

        self.dxH = self.dxHl + self.dxHnl

        self.gradH = list(self.Q()*sp.Matrix(self.xl())) + [sp.simplify(self.H.diff(self.xnl()[n])).doit() for n in range(self.nxnl())]
        #######################################################################

        print("Explicite Function")
        time.sleep(.1)
        MdxHnl = sp.Matrix.vstack(self.B2,self.B6)*sp.Matrix(list(self.dxHnl)) if self.nxnl()>0 else sp.zeros(self.nxl()+self.nwl(),1)
        Mznl = sp.Matrix.vstack(self.C2,self.C6)*sp.Matrix(list(self.znl())) if self.nwnl()>0 else sp.zeros(self.nxl()+self.nwl(),1)
        Mxl =sp.Matrix.vstack(self.A2,self.A6)*sp.Matrix(self.xl()) if self.nxl()>0 else sp.zeros(self.nxl()+self.nwl(),1)
        Mu = sp.Matrix.vstack(self.D2,self.D6)*self.Gains()*sp.Matrix(self.u) if self.ny()>0 else sp.zeros(self.nxl()+self.nwl(),1)
        self.Fl = parallel_factorize( list(Mxl + MdxHnl + Mznl + Mu)) if parallelize else list(Mxl + MdxHnl + Mznl + Mu)
        
        #######################################################################
        self.JJ = (self.J-self.J.T)/2
        self.RJ = (self.J+self.J.T)/2    
        
        #######################################################################
        if self.nxnl() + self.nwnl() > 0:
            print("Root Function")
            time.sleep(.1)
            self.isNL = True
            self.A = sp.Matrix.vstack( self.A4.as_mutable(), self.A5.as_mutable() )
            self.B = sp.Matrix.vstack( sp.Matrix.hstack(self.B4.as_mutable(),self.C4.as_mutable()), sp.Matrix.hstack(self.B5.as_mutable(),self.C5.as_mutable()) )
            self.C = sp.Matrix.vstack( self.D4.as_mutable(), self.D5.as_mutable() )
            Mxl = self.A*sp.Matrix(self.xl()) if self.nxl()>0 else sp.zeros(self.nxnl()+self.nwnl(),1)
            Mu = self.C*self.Gains()*sp.Matrix(self.u) if self.ny()>0 else sp.zeros(self.nxnl()+self.nwnl(),1)
            IF = list(sp.Matrix([dxnlm*self.fs for dxnlm in self.dxnl()]+self.wnl()) - self.B*sp.Matrix(list(self.dxHnl) + self.znl() ) - Mxl - Mu)
            self.ImpFunc = sp.Matrix(parallel_factorize(IF)) if parallelize else sp.Matrix(IF)
            #######################################################################
            print("Jacobian of Implicite Function")
            time.sleep(.1)
            JacFnl = sp.zeros(self.nxnl()+self.nwnl(),self.nxnl()+self.nwnl())
            nlvar = self.dxnl() + self.wnl()
            for n in range(self.nxnl()+self.nwnl()):
                for m in range(self.nxnl()+self.nwnl()):
                    JacFnl[n,m] = self.ImpFunc[n,0].diff(nlvar[m])
            self.JacFnl = sp.Matrix(JacFnl)
            print("Jacobian inverse")
            time.sleep(.1)
            self.iJacFnl = SymbolicGaussJordanInverse(sp.Matrix(JacFnl), SimplifyIn=False, SimplifyOut=False)
#            self.iJacFnl = sp.Matrix.inv(JacFnl, method='LU')
            print("\nImplicite Function")
            time.sleep(.1)
            Fnl =  list((-self.iJacFnl*sp.Matrix(self.ImpFunc)))
            self.Fnl = sp.Matrix(parallel_factorize(Fnl)) if parallelize else sp.Matrix(Fnl)
            print("Residual Function")
            time.sleep(.1)
            self.Fnl_residual = (sp.Matrix(self.ImpFunc).T*sp.Matrix(self.ImpFunc))[0,0]
        else:
            self.Fnl = []
            self.Fnl_residual = []
            self.isNL = False

        print("Output Function")
        time.sleep(.1)
        Fy =  list(self.Gains()*(self.Gx.T*sp.Matrix(self.dxH) + self.Gw.T*sp.Matrix(self.z) + self.Jy*self.Gains()*sp.Matrix(self.u)))
        self.Fy = sp.Matrix(parallel_factorize(Fy)) if parallelize else sp.Matrix(Fy)
        if np.matrix(self.RJ).sum() != 0:
            print("Power dissipated in structure")
            time.sleep(.1)
            self.PDJ = -(sp.Matrix(self.dxH + self.z + list(self.Gains()*sp.Matrix(self.u))).T*self.RJ*sp.Matrix(self.dxH + self.z + list(self.Gains()*sp.Matrix(self.u))))[0,0]
        
        if ExportCpp:
            print("Export C++ code")
            time.sleep(.1)
            self.writeCppCode()
        
    def Lambdify(self):
        print("Lambdify")
        time.sleep(.1)
        self.H_n = sp.lambdify(self.x + self.params, self.H, dummify=False, modules="numpy")
        self.z_n = sp.lambdify(self.x + self.w + self.params, self.z, dummify=False, modules="numpy")
        self.y_n = sp.lambdify(self.x + self.dx + self.w + self.u + self.params, list(self.Fy), dummify=False, modules="numpy")
        self.Fl_n = sp.lambdify(self.x + self.dxnl() + self.wnl() + self.u + self.params, list(self.Fl), dummify=False, modules = "numpy")        
        self.dxH_n = sp.lambdify(self.x + self.dx + self.params, list(self.dxH), dummify=False, modules = "numpy")
        self.PDJ_n = sp.lambdify(self.x + self.dx + self.w + self.u + self.params, self.PDJ, dummify=False, modules = "numpy")        
        if self.isNL:
            iJacFnl_n = []
            for n in range(self.nxnl()+self.nwnl()):
                line = []
                for m in range(self.nxnl()+self.nwnl()):
                    line.append(sp.lambdify((self.dxnl() + self.wnl() + self.xl() + self.xnl() + self.u + self.params), self.iJacFnl[n,m], dummify=False, modules = "numpy"))                    
                iJacFnl_n.append(line)
            self.iJacFnl_n = iJacFnl_n
            self.Fnl_n = sp.lambdify(self.dxnl() + self.wnl() + self.x + self.u + self.params, list(self.Fnl), dummify=False, modules = "numpy")
            self.Fnl_residual_n = sp.lambdify(self.dxnl() + self.wnl() + self.x + self.u + self.params, self.Fnl_residual, dummify=False, modules = "numpy")
            #######################################################################

    def Compose_Graphs(self,pHs2):
        self.H = self.H + pHs2.H
        self.x = self.x[:self.nxl()]+pHs2.x[:pHs2.nxl()]+self.x[self.nxl():]+pHs2.x[pHs2.nxl():]
        self.w = self.w[:self.nwl()]+pHs2.w[:pHs2.nwl()]+self.w[self.nwl():]+pHs2.w[pHs2.nwl():]
        self.z = self.z[:self.nwl()]+pHs2.z[:pHs2.nwl()]+self.z[self.nwl():]+pHs2.z[pHs2.nwl():]
        self.u = self.u + pHs2.u
        self.y = self.y + pHs2.y
        self.trans_u = self.trans_u + pHs2.trans_u
        self.trans_y = self.trans_y + pHs2.trans_y
        self.transformers = self.transformers + pHs2.transformers
        self.diagQ = self.diagQ + pHs2.diagQ
        self.diagR = self.diagR + pHs2.diagR
        self.diagG = self.diagG + pHs2.diagG
        self.params = self.params + pHs2.params
        self.unities['x'] += pHs2.unities['x']
        self.unities['w'] += pHs2.unities['w']
        self.unities['u'] += pHs2.unities['u']
        self.Graph.add_edges_from(pHs2.Graph.edges(data=True))
        self.list_of_edges = self.Graph.edges(data=True)
        self.subs += pHs2.subs
        self.subs_par += pHs2.subs_par

    def ShowGraph(self):
        pos=nx.circular_layout(self.Graph) 
        plt.figure()
        nx.draw_networkx_nodes(self.Graph,pos)
        nx.draw_networkx_edges(self.Graph,pos)
        nx.draw_networkx_labels(self.Graph,pos)
        edge_labels = {}
        for edge in self.list_of_edges:
            edge_labels[(edge[0], edge[1])] = edge[2]['ref']
        nx.draw_networkx_edge_labels(self.Graph, pos, labels = edge_labels)
        plt.show()


    def Build_Graph_from_Netlist(self,netlist):
        import Dictionary
        netlist_file = open(netlist + ".net", "r")
        self.netlist = list()
        with netlist_file as openfileobject:
            for line in openfileobject:
                # find first and last brackets for nodes definition
                nstart_nodes = line.find('[')
                nend_nodes = line.find(']')
                # Nodes list
                nodes_label = line[nstart_nodes+1:nend_nodes]
                # find first and last brackets for definition of arguments
                nstart_args = line.rfind('[')
                nend_args = line.rfind(']')
                # Arguments list
                arguments = line[nstart_args+1:nend_args]
                # Split component type and component label (first 2 elements of the line)
                splited_line = line[:nstart_nodes].split()
                component = splited_line[0]
                label = splited_line[1]
                # call the method for actual component            
                current_component = eval("Dictionary."+component+r"('"+label+r"',(" +nodes_label+"),("+arguments+',))')
                # Compose the system with the actual component
                self.Compose_Graphs(current_component)
                # save current component in a list
                self.netlist.append(current_component)
        # Close netlist file
        netlist_file.close()

    def buildStructurefromGraph(self):

        self.list_of_edges = self.Graph.edges(data=True)
        self.list_of_edge_labels = [ edge[2]['ref'] for edge in self.list_of_edges ]
        self.nodes_labels = self.Graph.nodes()

        # Compute incidence Matrix
        Gamma_init = -(nx.linalg.incidence_matrix(self.Graph, oriented=True)).todense()
        self.Gamma = np.matrix(np.zeros(Gamma_init.shape))
        self.Lambda = np.matrix(np.zeros(Gamma_init.shape))

        # Sort edges according to storage, dissipative, port and trans_port types        
        
        edges_index = []
        # Get storage edges
        self.list_of_realizabilities = []
        for n in range(self.nx()):
            edge_index = self.list_of_edge_labels.index(str(self.x[n]))
            edges_index += [edge_index]
            self.Gamma[:,n] = Gamma_init[:,edge_index]
            edge_realizability = self.list_of_edges[edge_index][2]['realizability']
            self.list_of_realizabilities.append(edge_realizability)
            if (edge_realizability == 'flux_controlled') | (edge_realizability == '?'):
                    self.Lambda[:,n] = np.abs(Gamma_init[:,edge_index])

        # Get disipative edges
        for n in range(self.nw()):
            edge_index = self.list_of_edge_labels.index(str(self.w[n]))
            edges_index += [edge_index]
            self.Gamma[:, self.nx() + n] = Gamma_init[:,edge_index]
            edge_realizability = self.list_of_edges[edge_index][2]['realizability']
            self.list_of_realizabilities.append(edge_realizability)
            if (edge_realizability == 'flux_controlled') | (edge_realizability == '?'):
                    self.Lambda[:,self.nx() +n] = np.abs(Gamma_init[:,edge_index])

        # Get port edges
        for n in range(self.ny()):
            edge_index = self.list_of_edge_labels.index(str(self.y[n]))
            edges_index += [edge_index]
            self.Gamma[:, self.nx() +self.nw()+ n] = Gamma_init[:,edge_index]
            edge_realizability = self.list_of_edges[edge_index][2]['realizability']
            self.list_of_realizabilities.append(edge_realizability)
            if (edge_realizability == 'flux_controlled') | (edge_realizability == '?'):
                    self.Lambda[:,self.nx() +self.nw()+n] = np.abs(Gamma_init[:,edge_index])
        
        # Get trans_port edges
        for n in range(self.ntrans()):
            edge_index = self.list_of_edge_labels.index(str(self.trans_y[n]))
            edges_index += [edge_index]
            self.Gamma[:, self.nx() +self.nw()+self.ny()+ n] = Gamma_init[:,edge_index]
            edge_realizability = self.list_of_edges[edge_index][2]['realizability']
            self.list_of_realizabilities.append(edge_realizability)
            if (edge_realizability == 'flux_controlled') | (edge_realizability == '?'):
                    self.Lambda[:,self.nx() +self.nw()+self.ny()+n] = np.abs(Gamma_init[:,edge_index])

        # Put 0 node at the top of the node list
        index_datum_node = self.nodes_labels.index(0) if any([0 == nod for nod in self.nodes_labels]) else []
        ordered_node_index = [index_datum_node] + range(index_datum_node) + range(index_datum_node+1, self.nodes_labels.__len__())
        # sort
        self.Gamma = self.Gamma[ordered_node_index,:]
        self.Lambda = self.Lambda[ordered_node_index,:]
        self.nodes_labels = [self.nodes_labels[n] for n in ordered_node_index]
        
        # Sort edges
        self.list_of_edges = [self.list_of_edges[el] for el in edges_index]
        self.list_of_edge_labels = [edge[2]['ref'] for edge in self.list_of_edges]
        self.list_of_types = [edge[2]['type'] for edge in self.list_of_edges]
        self.list_of_link_refs = [edge[2]['link_ref'] for edge in self.list_of_edges]
    
        rowindexGamma = lambda c: [e[0,0] for e in np.nonzero(self.Gamma[:,c])[0].T]
        colindexGamma = lambda r: [e[0,0] for e in np.nonzero(self.Gamma[r,:])[1].T]
        CompareMatrix = lambda M1,M2: not (M1-M2).any()
        
        nb = self.list_of_edge_labels.__len__()
        nn = self.nodes_labels.__len__()

        #force realizability for 0 node
        self.Lambda[0,:] = 0

        branches_index = range(nb)
        nodes_index = range(1,nn)
        Lambda_iter = np.zeros(self.Lambda.shape)
        Error = False

        # Realizability analysis on the branches_index
        while (branches_index.__len__() > 0) & (not Error):

            # Is change?
            while not CompareMatrix(Lambda_iter,self.Lambda):
                
                # Init Lambda
                Lambda_iter = np.matrix(self.Lambda, copy=True)

                # Iteration sur les branches
                for e in branches_index:

                    if branches_index.__len__() == 0:
                        break

                    elif any([e==el for el in branches_index]):
                        
                        # Get edge values in Lambda
                        Lambda_values = [el[0,0] for el in self.Lambda[rowindexGamma(e),e] ]
                        realizability = self.list_of_realizabilities[e]
                        edge_type = self.list_of_types[e]

                        # Effort Controlled
                        if realizability =='effort_controlled':
                                self.Lambda[:, e] = 0
                                # remove the edge from edge list
                                indice_ctrl_edge_in_branches_index = branches_index.index(e)
                                branches_index = branches_index[:indice_ctrl_edge_in_branches_index] + branches_index[indice_ctrl_edge_in_branches_index+1:]                        
    
                        # Flux Controlled
                        elif realizability =='flux_controlled':
                            if sum(Lambda_values)==0:
                                print("Error: Realizability of "+self.list_of_edge_labels[e]+" does not coincide with definition")
                                Error = True
                                break
                            elif sum(Lambda_values)==1:
                                # set other nodes to 0
                                indice_ctrl_node = [el[0,0] for el in self.Lambda[:,e]].index(1)
                                # remove the edge from edge list
                                indice_ctrl_edge_in_branches_index = branches_index.index(e)
                                branches_index = branches_index[:indice_ctrl_edge_in_branches_index] + branches_index[indice_ctrl_edge_in_branches_index+1:]                        
                                # remove the node from nodes list
                                nodes_index = nodes_index[:indice_ctrl_node] + nodes_index[indice_ctrl_node+1:]                        
                                self.Lambda[indice_ctrl_node, :] = 0
                                self.Lambda[indice_ctrl_node, e] = 1
                                
                        # Undefined Controlled
                        elif realizability =='?':
                            # Test for effort Controlled
                            if sum(Lambda_values)==0:
                                self.list_of_realizabilities[e] = 'effort_controlled'
                                # remove the edge from edge list
                                indice_ctrl_edge_in_branches_index = branches_index.index(e)
                                branches_index = branches_index[:indice_ctrl_edge_in_branches_index] + branches_index[indice_ctrl_edge_in_branches_index+1:]                        
                                # Link Transformers
                                if edge_type == 'trans_port':
                                    transformer_index = [any([str(el)==str(self.list_of_edge_labels[e]) for el in ell['y']]) for ell in self.transformers].index(True)
                                    transformer = self.transformers[transformer_index]
                                    linked_edge_index = self.list_of_edge_labels.index(self.list_of_link_refs[e])
                                    self.transformers[transformer_index]['alpha'] = transformer['alpha']**-1
                                    if transformer['type'] == 'gyrator':                                    
                                        if self.list_of_realizabilities[linked_edge_index] == '?' :
                                            self.list_of_realizabilities[linked_edge_index] = 'effort_controlled'
                                            # remove the edge from edge list
                                            indice_ctrl_edge_in_branches_index = branches_index.index(linked_edge_index)
                                            branches_index = branches_index[:indice_ctrl_edge_in_branches_index] + branches_index[indice_ctrl_edge_in_branches_index+1:]                        
                                            self.Lambda[:,linked_edge_index] = 0
                                        elif self.list_of_realizabilities[linked_edge_index] == 'flux_controlled' :
                                            print("Error: Realizability of transformer (" + self.list_of_edge_labels[e] + ', ' + self.list_of_link_refs[e] + ") does not coincide with definition")
                                            Error = True
                                            break
                                    if transformer['type'] == 'transformer':                                    
                                        if self.list_of_realizabilities[linked_edge_index] == '?' :
                                            self.list_of_realizabilities[linked_edge_index] = 'flux_controlled'
                                        elif self.list_of_realizabilities[linked_edge_index] == 'effort_controlled' :
                                            print("Error: Realizability of transformer (" + self.list_of_edge_labels[e] + ', ' + self.list_of_link_refs[e] + ") does not coincide with definition")
                                            Error = True
                                            break
                                            
                            # Test for flux Controlled
                            elif sum(Lambda_values)==1:
                                self.list_of_realizabilities[e] = 'flux_controlled'
                                # set other nodes to 0
                                indice_ctrl_node = [el[0,0] for el in self.Lambda[:,e]].index(1)
                                # remove the edge from edge list
                                indice_ctrl_edge_in_branches_index = branches_index.index(e)
                                branches_index = branches_index[:indice_ctrl_edge_in_branches_index] + branches_index[indice_ctrl_edge_in_branches_index+1:]                        
                                # remove the node from nodes list
                                nodes_index = nodes_index[:indice_ctrl_node] + nodes_index[indice_ctrl_node+1:]                        
                                self.Lambda[indice_ctrl_node, branches_index] = 0
                                self.Lambda[indice_ctrl_node, e] = 1
                                # Link Transformers
                                if edge_type == 'trans_port':
                                    transformer_index = [any([str(el)==str(self.list_of_edge_labels[e]) for el in ell['y']]) for ell in self.transformers].index(True)
                                    transformer = self.transformers[transformer_index]
                                    linked_edge_index = self.list_of_edge_labels.index(self.list_of_link_refs[e])
                                    if transformer['type'] == 'gyrator':                                    
                                        if self.list_of_realizabilities[linked_edge_index] == '?' :
                                            self.list_of_realizabilities[linked_edge_index] = 'flux_controlled'
                                        elif self.list_of_realizabilities[linked_edge_index] == 'effort_controlled' :
                                            print("Error: Realizability of transformer (" + self.list_of_edge_labels[e] + ', ' + self.list_of_link_refs[e] + ") does not coincide with definition")
                                            Error = True
                                            break
                                    if transformer['type'] == 'transformer':
                                        if self.list_of_realizabilities[linked_edge_index] == '?' :
                                            self.list_of_realizabilities[linked_edge_index] = 'effort_controlled'
                                            # remove the edge from edge list
                                            indice_ctrl_edge_in_branches_index = branches_index.index(linked_edge_index)
                                            branches_index = branches_index[:indice_ctrl_edge_in_branches_index] + branches_index[indice_ctrl_edge_in_branches_index+1:]                        
                                            self.Lambda[:,linked_edge_index] = 0
                                        elif self.list_of_realizabilities[linked_edge_index] == 'flux_controlled' :
                                            print("Error: Realizability of transformer (" + self.list_of_edge_labels[e] + ', ' + self.list_of_link_refs[e] + ") does not coincide with definition")
                                            Error = True
                                            break
    
                    # Iteration sur les noeuds
                    for n in nodes_index:
                        Lambda_values = [el[0,0] for el in self.Lambda[n,colindexGamma(n)].T ]                    
                        if sum(Lambda_values) == 0:
                            print(Lambda_values)
                            print("Error: Realizability conflict on node " + str(self.nodes_labels[n]) + ".")
                            Error = True
                            break
                        elif sum(Lambda_values) == 1:
                            indice_ctrl_edge = [el[0,0] for el in self.Lambda[n,:].T].index(1)
                            if self.list_of_realizabilities[indice_ctrl_edge] == '?':
                                self.list_of_realizabilities[indice_ctrl_edge] = 'flux_controlled'
                                self.Lambda[:,indice_ctrl_edge] = 0
                                self.Lambda[n, indice_ctrl_edge] = 1
                                # Link Transformers
                                if self.list_of_types[indice_ctrl_edge] == 'trans_port':
                                    transformer_index = [ any([self.list_of_edge_labels[indice_ctrl_edge]==str(el) for el in trans['y']]) for trans in self.transformers].index(True)
                                    transformer = self.transformers[transformer_index]
                                    linked_edge_index = self.list_of_edge_labels.index(self.list_of_link_refs[indice_ctrl_edge])
                                    if transformer['type'] == 'gyrator':                                    
                                        if self.list_of_realizabilities[linked_edge_index] == '?' :
                                            self.list_of_realizabilities[linked_edge_index] = 'flux_controlled'
                                        elif self.list_of_realizabilities[linked_edge_index] == 'effort_controlled' :
                                            print("Error: Realizability of transformer (" + self.list_of_edge_labels[indice_ctrl_edge] + ', ' + self.list_of_link_refs[indice_ctrl_edge] + ") does not coincide with definition")
                                            Error = True
                                            break
                                    if transformer['type'] == 'transformer':                                    
                                        if self.list_of_realizabilities[linked_edge_index] == '?' :
                                            self.list_of_realizabilities[linked_edge_index] = 'effort_controlled'
                                            # remove the edge from edge list
                                            indice_ctrl_edge_in_branches_index = branches_index.index(linked_edge_index)
                                            branches_index = branches_index[:indice_ctrl_edge_in_branches_index] + branches_index[indice_ctrl_edge_in_branches_index+1:]                        
                                            self.Lambda[:,linked_edge_index] = 0
                                        elif self.list_of_realizabilities[linked_edge_index] == 'flux_controlled' :
                                            print("Error: Realizability of transformer (" + self.list_of_edge_labels[indice_ctrl_edge] + ', ' + self.list_of_link_refs[indice_ctrl_edge] + ") does not coincide with definition")
                                            Error = True
                                            break
    
                                if any([indice_ctrl_edge is el for el in branches_index]):
                                    indice_ctrl_edge_in_branches_index = branches_index.index(indice_ctrl_edge)
                                    branches_index = branches_index[:indice_ctrl_edge_in_branches_index] + branches_index[indice_ctrl_edge_in_branches_index+1:]                        
                            elif self.list_of_realizabilities[indice_ctrl_edge] == 'flux_controlled':
                                self.Lambda[:,indice_ctrl_edge] = 0
                                self.Lambda[n, indice_ctrl_edge] = 1                 
                                if any([indice_ctrl_edge is el for el in branches_index]):
                                    indice_ctrl_edge_in_branches_index = branches_index.index(indice_ctrl_edge)
                                    branches_index = branches_index[:indice_ctrl_edge_in_branches_index] + branches_index[indice_ctrl_edge_in_branches_index+1:]                        

            if branches_index.__len__() > 0:
                print("Error: Loop in realizability analysis")
                print("branches_index:")
                print(branches_index)
                print(CompareMatrix(Lambda_iter,self.Lambda))
                break
                Error = True

        self.Gamma = self.Gamma[1:,:]
        self.Lambda = self.Lambda[1:,:]
        nb_nodes = self.nodes_labels.__len__()-1
        nb_edges = self.list_of_edge_labels.__len__()
        is_flux_controlled = self.Lambda.sum(axis=0)

        M_I = sc.diag([is_flux_controlled[0,n] for n in range(nb_edges)])
        M_V = np.eye(M_I.shape[1])-M_I
        Mp1 = np.concatenate((np.zeros((nb_nodes,nb_edges)),M_V,M_I))
        Mp2 = np.concatenate((np.eye(nb_nodes),np.zeros((nb_edges,nb_nodes)),np.zeros((nb_edges,nb_nodes))))
        Mp3 = np.concatenate((np.zeros((nb_nodes,nb_edges)),M_I,M_V))
        Permut = np.matrix(np.concatenate((Mp2, Mp1, Mp3), axis=1))
                
        M1 = np.concatenate((self.Gamma.T,np.zeros((nb_nodes,nb_nodes))))
        M2 = np.concatenate((-np.eye(nb_edges),np.zeros((nb_nodes,nb_edges))))
        M3 = np.concatenate((np.zeros((nb_edges,nb_edges)),self.Gamma))
        GG = np.concatenate((M1,M2,M3),axis=1)
        GG = GG*Permut
        
        def op1(M, i ,j):
            temp = [M[i,n] for n in np.arange(M.shape[1])] 
            M[i,:] = M[j,:]
            M[j,:] = temp
            return M
        def op2(M,i,a):
            M[i,:] = [a*M[i,n] for n in np.arange(M.shape[1])] 
            return M
            
        def op3(M,i,j,a):
            M[i,:] = [M[i,n]+a*M[j,n] for n in np.arange(M.shape[1])] 
            return M
            
        for n in range(GG.shape[0]):
            temp = np.nonzero(GG[n:,n]!=0)[0]
            l = n + temp[0,0] if temp.shape[1]>0 else n
            GG=op1(GG, n ,l)
            GG=op2(GG,n,GG[n,n]**-1)
            for m in np.arange(GG.shape[0]):
                if m != n :
                    if GG[m,n] !=0:
                        GG=op3(GG,m,n,-GG[m,n]**-1)
        
        J = -sp.Matrix(GG[nb_nodes:,nb_nodes+nb_edges:])
        
        if self.ntrans()>0:
            self.Gx_trans = J[:self.nx(), self.nx() + self.nw() + self.ny() : ]
            self.Gw_trans  = J[self.nx():self.nx()+self.nw(),self.nx() + self.nw() + self.ny() :]
            self.Gy_trans = J[self.nx()+self.nw():self.nx()+self.nw()+ self.ny(),self.nx() + self.nw() + self.ny() : ]        
            self.Jtrans_trans = J[self.nx()+self.nw()+ self.ny():,self.nx() + self.nw() + self.ny():]
            switch_list = [trans['alpha'] * sp.Matrix([[0,1],[-1,0]]) for trans in self.transformers]
            Mswitch = sc.linalg.block_diag(*switch_list)
            G_trans = sp.Matrix(J[:self.nx()+self.nw()+ self.ny(),self.nx() + self.nw() + self.ny():])
            self.Jtrans_trans = G_trans * Mswitch * G_trans.T
        else:
            self.Jtrans_trans = sp.zeros(self.nx()+self.nw()+ self.ny(),self.nx()+self.nw()+ self.ny())

        J = sp.Matrix(J[:self.nx()+self.nw()+ self.ny(), :self.nx()+self.nw()+ self.ny()]) + self.Jtrans_trans
            
        self.Jx = J[:self.nx(),:self.nx()]
        self.K= -J[:self.nx(),self.nx():self.nx()+self.nw()]    
        self.Gx = J[:self.nx(),self.nx()+self.nw():self.nx()+self.nw()+self.ny()]
        self.Jw = J[self.nx():self.nx()+self.nw(),self.nx():self.nx()+self.nw()]
        self.Gw = J[self.nx():self.nx()+self.nw(),self.nx()+self.nw():self.nx()+self.nw()+self.ny()]
        self.Jy = -J[self.nx()+self.nw():self.nx()+self.nw()+self.ny(),self.nx()+self.nw():self.nx()+self.nw()+self.ny()]        
        self.J = J
        if bool(Error) |  bool(np.matrix(self.J+self.J.T).sum()):
            print("    !!!!    Error in realizability analysis    !!!!")           
                            
#######################################################################

    def simulation(self, SeqU, SeqP=None, x0=None, fs=48000, NumTol=eps, \
                   ListConstParams=None, Rebuild=True, NbItNR=100, NumTolNr=1e-12,\
                   language='py'):

        if Rebuild:
            self.Build(fs=fs)

        # languages = 'py' or 'cpp'
        if language == 'c++':
            self.writeFilesData(SeqU=SeqU,SeqP=SeqP)
            print(""); print("Execute Cpp code"); print("")
            print("Press 'return' two times when done...")
            raw_input() 
            self.getFilesData(fs)
            self.PlotPowerBal(fs=fs)

        if language == 'py':
            print("\nSimulation")
            time.sleep(1)

            Te = fs**(-1)
            nt = SeqU.__len__()
            self.nt = nt        
                    
            self.Lambdify()
            #######################################################################
    
            x = [0,]*self.nx() if x0 == None else x0
            dx = [0,]*self.nx()
            w = [0,]*self.nw()
    
            if (SeqP is None) and (ListConstParams is not None):
                SeqP=ListConstParams*nt
            elif (SeqP is None) and (ListConstParams is None):
                SeqP = [[fs],]*nt if self.params.__len__() > 0 else [[],]*nt
    
            self.SeqX = []
            self.SeqX.append(x)
    
            self.SeqDtX = []
            self.SeqDxH = []
    
            self.SeqW = []
            self.SeqZ = []
    
            self.SeqY = []
            self.SeqU = SeqU
    
            self.SeqP = SeqP
    
            self.SeqDtE = []
            self.SeqPD = []
            self.SeqH = []
            self.SeqPS = []
    
            nxl = self.nxl()
            nxnl = self.nxnl()
            nwl = self.nwl()
    
            widgets = ['Simulation of '+ self.label, pb.Percentage(), ' ', pb.ETA()]
            pbar = pb.ProgressBar(widgets=widgets, maxval=nt).start()
    
    
            for k in range(nt):
    
                u = SeqU[k]
                pars = SeqP[k]
    
                dxl = dx[:nxl]
                dxnl = dx[nxl:]
    
                wnl = w[nwl:]
    
                ###################################################################
                
                if self.isNL:
                    VarNL = dxnl + wnl
                    it = 0
                    step = 1
                    res = 1
                    while ((it<NbItNR) & (res>NumTolNr) & (step>eps**2)):
    #                    VarNL_init = VarNL
                        VarNL = [e1+e2 for (e1,e2) in zip(VarNL, self.Fnl_n(*(VarNL + x + u + pars)))] 
                        it+=1
                        res = self.Fnl_residual_n(*(VarNL + x + u + pars))
    #                    step = np.sqrt(sum([e1-e2 for (e1, e2) in zip(VarNL, VarNL_init)]))
                    dxnl, wnl = VarNL[:nxnl], VarNL[nxnl:]
    
                ###################################################################
                Varl = self.Fl_n(*(x + dxnl + wnl + u + pars))
                dxl, wl = Varl[:nxl], Varl[nxl:]
                ###################################################################
                dx, w = dxl + dxnl, wl + wnl
                ###################################################################
                z = self.z_n(*(x + w + pars))
                dxH = self.dxH_n( *(x + dx + pars) )
                ###################################################################
                y = self.y_n(*(x + dx + w + u + pars))
                ###################################################################
                x = [sum(pair) for pair in zip(x, dx)]
                ###################################################################
                PdInStructure = self.PDJ_n(*(x + dx + w + u + pars))
                ###################################################################            
                self.SeqX.append(x)
                self.SeqDtX.append([e/float(Te) for e in dx])
                self.SeqDxH.append(dxH)
                self.SeqW.append(w)
                self.SeqZ.append(z)        
                self.SeqY.append(y)
                self.SeqH.append(self.H_n(*x+pars))
                self.SeqPD.append(sum([e1*e2 for (e1, e2) in zip(z, w)]) +PdInStructure)
                self.SeqPS.append(sum([e1*e2 for (e1, e2) in zip(y, u)]))
                self.SeqDtE.append(sum([e1*e2/float(Te) for (e1, e2) in zip(dxH, dx)]))
                #######################################################################
                self.nt = k
                pbar.update(k+1)
            #######################################################################
            plt.close('all')
            self.PlotPowerBal(fs=fs)

    def writeCppCode(self):
        cfile(self)
        hfile(self)
        MainCpp(self)

    def writeFilesData(self, SeqU=0, SeqP=None, x0=[]):
        self.nt = SeqU.__len__() if type(SeqU)==list else SeqU
        self.SeqP = [[],]*self.nt if SeqP == None else SeqP
        str_in = ""
        for n in range(self.nt):
            for m in range(self.ny()):
                str_in += str(SeqU[n][m])
                str_in += " " if m<self.ny() else ""
            str_in += "\n"
        in_file = open(self.folder+"/u.txt", 'w')
        in_file.write(str_in)
        in_file.close()        

        str_p = ""
        for n in range(self.nt):
            for m in range(self.np()):
                str_p += str(SeqP[n][m])
                str_p += " " if m<self.np() else ""
            str_p += "\n"
        p_file = open(self.folder+"/p.txt", 'w')
        p_file.write(str_p)
        p_file.close()
        x0 = [0,]*self.nx() if x0.__len__()==0 else x0
        str_x0=""
        for e in x0:
            str_x0 += str(e) + " "
        x0_file = open(self.folder+"/x0.txt", 'w')
        x0_file.write(str_x0)
        x0_file.close()
        
        
    def getFilesData(self, fs):

        out_file = open(self.folder+"/u.txt", 'r')
        str_out = out_file.read()
        out_file.close()
        self.SeqU = [ [float(e) for e in E.split()] for E in str_out.split('\n')[:-1]]

        out_file = open(self.folder+"/y.txt", 'r')
        str_out = out_file.read()
        out_file.close()
        self.SeqY = [ [float(e) for e in E.split()] for E in str_out.split('\n')[:-1]]

        out_file = open(self.folder+"/x.txt", 'r')
        str_out = out_file.read()
        out_file.close()
        self.SeqX = [ [float(e) for e in E.split()] for E in str_out.split('\n')[:-1]]

        out_file = open(self.folder+"/dx.txt", 'r')
        str_out = out_file.read()
        out_file.close()
        self.SeqDtX = [ [float(e)*fs for e in E.split()] for E in str_out.split('\n')[:-1]]

        out_file = open(self.folder+"/dxH.txt", 'r')
        str_out = out_file.read()
        out_file.close()
        self.SeqDxH = [ [float(e) for e in E.split()] for E in str_out.split('\n')[:-1]]

        out_file = open(self.folder+"/w.txt", 'r')
        str_out = out_file.read()
        out_file.close()
        self.SeqW = [ [float(e) for e in E.split()] for E in str_out.split('\n')[:-1]]

        out_file = open(self.folder+"/z.txt", 'r')
        str_out = out_file.read()
        out_file.close()
        self.SeqZ = [ [float(e) for e in E.split()] for E in str_out.split('\n')[:-1]]
        
        self.SeqPS = [sum([e1*e2 for (e1, e2) in zip(u, y)]) for (u, y) in zip(self.SeqU, self.SeqY)]
        self.SeqPD = [sum([e1*e2 for (e1, e2) in zip(z, w)]) for (w, z) in zip(self.SeqW, self.SeqZ)]
        self.SeqDtE = [sum([e1*e2 for (e1, e2) in zip(dtx, dxH)]) for (dtx, dxH) in zip(self.SeqDtX, self.SeqDxH)]
        
#######################################################################

    def replaceParByExpr(self, subs=[]):
        self.subs_par += subs
        while self.subs_par.__len__()>0:
            symb, expr = self.subs_par.pop()

            index_par = self.params.index(symb)
            
            self.x = [sp.sympify(el).subs(symb,expr) for el in self.x]            
            self.w = [sp.sympify(el).subs(symb,expr) for el in self.w]            
            self.z = [sp.sympify(el).subs(symb,expr) for el in self.z]            
            self.u = [sp.sympify(el).subs(symb,expr) for el in self.u]            
            self.y = [sp.sympify(el).subs(symb,expr) for el in self.y]            
            self.params = self.params[:index_par]+self.params[index_par+1:]
            self.diagQ = [sp.sympify(el).subs(symb,expr) for el in self.diagQ]            
            self.diagR = [sp.sympify(el).subs(symb,expr) for el in self.diagR]            
            self.H = sp.sympify(self.H).subs(symb,expr)
            self.J = self.J.subs(symb,expr)
            J = self.J
            self.Jx = J[:self.nx(),:self.nx()]
            self.K= -J[:self.nx(),self.nx():self.nx()+self.nw()]    
            self.Gx = J[:self.nx(),self.nx()+self.nw():self.nx()+self.nw()+self.ny()]
            self.Jw = J[self.nx():self.nx()+self.nw(),self.nx():self.nx()+self.nw()]
            self.Gw = J[self.nx():self.nx()+self.nw(),self.nx()+self.nw():self.nx()+self.nw()+self.ny()]
            self.Jy = -J[self.nx()+self.nw():self.nx()+self.nw()+self.ny(),self.nx()+self.nw():self.nx()+self.nw()+self.ny()]        
            self.PDJ = self.PDJ.subs(symb,expr)
            self.Fl = [el.subs(symb,expr) if type(el)==sp.Expr else el for el in  self.Fl]
            self.Fnl = [el.subs(symb,expr) if type(el)==sp.Expr else el for el in  self.Fnl]
            self.dxH = [el.subs(symb,expr) if type(el)==sp.Expr else el for el in  self.Fl]
            self.gradH = [el.subs(symb,expr) if type(el)==sp.Expr else el for el in  self.Fnl]


    def applySubs(self, subs=[]):
        self.subs += subs
        while self.subs.__len__()>0:
            symb, expr = self.subs.pop()
            self.H = self.H.subs(symb,expr)
            self.x = [el.subs(symb,expr) for el in  self.x]
            self.w = [el.subs(symb,expr) for el in self.w]
            self.z = [el.subs(symb,expr) for el in self.z]
            self.u = [el.subs(symb,expr) for el in self.u]
            self.y = [el.subs(symb,expr) for el in self.y]
            self.params = [el.subs(symb,expr) if type(el)==sp.Expr else el for el in self.params]
            self.diagQ = [el.subs(symb,expr)  if type(el)==sp.Expr else el for el in self.diagQ]
            self.diagR = [el.subs(symb,expr)  if type(el)==sp.Expr else el for el in self.diagR]
            self.Fl = [el.subs(symb,expr) for el in  self.Fl]
            self.Fnl = [el.subs(symb,expr) for el in  self.Fnl]
            self.dxH = [el.subs(symb,expr) if type(el)==sp.Expr else el for el in  self.Fl]
            self.gradH = [el.subs(symb,expr) if type(el)==sp.Expr else el for el in  self.Fnl]
            self.PDJ = self.PDJ.subs(symb,expr)
            self.J = self.J.subs(symb,expr)
            J = self.J
            self.Jx = J[:self.nx(),:self.nx()]
            self.K = -J[:self.nx(),self.nx():self.nx()+self.nw()]    
            self.Gx = J[:self.nx(),self.nx()+self.nw():self.nx()+self.nw()+self.ny()]
            self.Jw = J[self.nx():self.nx()+self.nw(),self.nx():self.nx()+self.nw()]
            self.Gw = J[self.nx():self.nx()+self.nw(),self.nx()+self.nw():self.nx()+self.nw()+self.ny()]
            self.Jy = -J[self.nx()+self.nw():self.nx()+self.nw()+self.ny(),self.nx()+self.nw():self.nx()+self.nw()+self.ny()]        

    def ExportWav(self, tupple, label):


        # data
        var, ind = tupple
        sig = eval("[e["+str(ind)+"] for e in self.Seq"+var+"]")

        from struct import pack
        import wave
        
        ## GENERATE MONO FILE ##
        wv = wave.open(self.folder+'/'+label+'.wav', 'w')
        wv.setparams((1, 2, self.fs, 0, 'NONE', 'not compressed'))
        maxVol=2**15-1.0 #maximum amplitude
        Normalisation = np.max(np.abs(sig))
        wvData=""
        for i in range(0, sig.__len__()):
                wvData+=pack('h', maxVol*sig[i]/Normalisation)
        wv.writeframes(wvData)
        wv.close()

    def TransferFunction(self,tupple1,tupple2,fs, nfft=65536):
        """
        usage: TransfertFunction(tupple1,tupple2,fs)
        with tupple1 = (string, int) so that signal 1 is phs.string[int]    """

        import scipy.signal as sig
        from TOOLS import MyPlot2

        var1, ind1 = tupple1
        var2, ind2 = tupple2
        s1 = eval("[e["+str(ind1)+"] for e in self.Seq"+var1+"]")
        s2 = eval("[e["+str(ind2)+"] for e in self.Seq"+var2+"]")
        f, Pxx_den1 = sig.welch(s1, fs, nperseg=nfft)
        f, Pxx_den2 = sig.welch(s2, fs, nperseg=nfft)
        nfmax = np.nonzero(f>20e3)[0][0]
        nfmin = np.nonzero(f>10)[0][0]
        plt.figure()
        TF = Pxx_den1/Pxx_den2
        f = f[nfmin:nfmax]
        TF = [20*np.log10(el) for el in TF[nfmin:nfmax]]

        str1 = str(eval("self."+var1.lower()+'['+str(ind1)+']'))
        str2 = str(eval("self."+var2.lower()+'['+str(ind2)+']'))

        datax = [f]
        datay = [TF]
        label = [r'$20\mbox{log}_{10}('+str2+'(f)/'+str1+'(f))$']
        unitx = ['Hz']
        limits = [None]
        
        unitx = r'$f$ (Hz)'
        unity = r'Transfer function (dB)'
        filelabel = self.folder+'/transfer'+str1+'To'+str2
        maintitle = 'Transfer function $' + str1+'$ to $'+str2 + '$'
        figsize = (figwidth, 6)
        MyPlot2(datax, datay, label, unitx, unity, limits, fontsize=fontsize-5, \
                    legendfontsize=legendfontsize-10, linewidth=linewidth, figsize=figsize, axedef=[.17,.16,.75,.72], \
                    filelabel=filelabel, maintitle=maintitle, log = 'x')

        return f, TF

    def PrintLatex(self,mode='equation*'):
        str_latex = """ 
\documentclass[11pt, oneside]{article}   	% use "amsart" instead of "article" for AMSLaTeX format
\usepackage{geometry}                		% See geometry.pdf to learn the layout options. There are lots.
\geometry{letterpaper}                   		% ... or a4paper or a5paper or ... 
%\geometry{landscape}                		% Activate for for rotated page geometry
%\usepackage[parfill]{parskip}    		% Activate to begin paragraphs with an empty line rather than an indent
\usepackage{graphicx}				% Use pdf, png, jpg, or eps§ with pdflatex; use eps in DVI mode
								% TeX will automatically convert eps --> pdf in pdflatex		
\usepackage{amssymb}
\\title{Structure of pHs \\texttt{"""+self.label+"""}}
\\author{A. Falaize}
%\date{}							% Activate to display a given date or no date

\\begin{document}
\maketitle
\section{System dimensions}

"""
        str_latex += "    \n"
        str_latex += "    $$ n_\mathbf{x} = "+sp.printing.latex(self.nx(),fold_short_frac=True, mat_str = "array")+' $$ \n'
        str_latex += "    $$ n_\mathbf{w} = "+sp.printing.latex(self.nw(),fold_short_frac=True, mat_str = "array")+' $$ \n'
        str_latex += "    $$ n_\mathbf{y} = "+sp.printing.latex(self.ny(),fold_short_frac=True, mat_str = "array")+' $$ \n'
        str_latex += "    $$ n_\mathbf{p} = "+sp.printing.latex(self.np(),fold_short_frac=True, mat_str = "array")+' $$ \n'
        if self.nx()>0:
            str_latex += "    \section{System variables}\n\n $$ \mathbf{x} = "
            str_latex += ""+sp.printing.latex(sp.Matrix(self.x),fold_short_frac=True, mat_str = "array")+' $$ \n'
            str_latex += "    $$ \n \mathbf{H} = "
            str_latex += "    " + sp.printing.latex(self.H,fold_short_frac=True, mat_str = "array")+'$$ \n'
            str_latex += "    $$\n \\nabla \mathbf{H} = "
            str_latex += "    " + sp.printing.latex(sp.Matrix(self.gradH),fold_short_frac=True, mat_str = "array")+'$$ \n'
        if self.nw()>0:
            str_latex += "    $$ \n \mathbf{w} = "
            str_latex += "    " + sp.printing.latex(sp.Matrix(self.w),fold_short_frac=True, mat_str = "array")+'$$ \n'
            str_latex += "    $$ \n \mathbf{z} = "
            str_latex += "    " + sp.printing.latex(sp.Matrix(self.z),fold_short_frac=True, mat_str = "array")+'$$ \n'
        if self.ny()>0:
            str_latex += "    $$ \n \mathbf{u} = "
            str_latex += "    " + sp.printing.latex(sp.Matrix(self.u),fold_short_frac=True, mat_str = "array")+'$$ \n'
            str_latex += "    $$ \n \mathbf{y} = "
            str_latex += "    " + sp.printing.latex(sp.Matrix(self.y),fold_short_frac=True, mat_str = "array")+'$$ \n'            
        if self.np()>0:
            str_latex += "    $$ \n \mathbf{p} = "
            str_latex += "    " + sp.printing.latex(sp.Matrix(self.params),fold_short_frac=True, mat_str = "array")+'$$ \n'
        str_latex += "    \section{System structure}\n\n $$ \n \mathbf{J_x} = "
        str_latex += "    " + sp.printing.latex(self.Jx,fold_short_frac=True, mat_str = "array")+'$$ \n'
        str_latex += "    $$ \n \mathbf{K} = "
        str_latex += "    " + sp.printing.latex(self.K,fold_short_frac=True, mat_str = "array")+'$$ \n'
        str_latex += "    $$ \n \mathbf{G_x} = "
        str_latex += "    " + sp.printing.latex(self.Gx,fold_short_frac=True, mat_str = "array")+'$$ \n'
        str_latex += "    $$ \n \mathbf{Jw} = "
        str_latex += "    " + sp.printing.latex(self.Jw,fold_short_frac=True, mat_str = "array")+'$$ \n'
        str_latex += "    $$ \n \mathbf{G_w} = "
        str_latex += "    " + sp.printing.latex(self.Gw,fold_short_frac=True, mat_str = "array")+'$$ \n'
        str_latex += "    $$ \n \mathbf{Jy} = "
        str_latex += "    " + sp.printing.latex(self.Jy,fold_short_frac=True, mat_str = "array")+'$$ \n'
        str_latex += "    $$ \n \mathbf{J} = "
        str_latex += "    " + sp.printing.latex(self.J,fold_short_frac=True, mat_str = "array")+'$$ \n'
        str_latex += "    " + "\n\end{document}"
        latex_file = open(self.folder+"/phs_latex.tex", 'w')
        latex_file.write(str_latex)
        latex_file.close()
        
        
    def PlotPowerBal(self, EndView = 0, fs=48000):
        EndView = EndView if EndView is not 0 else self.nt
        t = np.linspace(0,float((EndView-1))/fs, EndView)
        #######################################################################
        plt.figure()                
        plt.subplot(4,1,1)
        plt.plot(t,map(lambda x: float(x), self.SeqDtE[:EndView]), label = 'DtE')
        plt.legend(loc = 0)
        plt.subplot(4,1,2)
        plt.plot(t,map(lambda x: float(x), self.SeqPD[:EndView]), label = 'Pd')
        plt.legend(loc = 0)
        plt.subplot(4,1,3)
        plt.plot(t,map(lambda x: float(x), self.SeqPS[:EndView]), label = 'Ps')
        plt.legend(loc = 0)
        plt.subplot(4,1,4)
        plt.plot(t,map(lambda x,y,z: float(x)+float(y)-float(z), self.SeqDtE[:EndView],self.SeqPD[:EndView],self.SeqPS[:EndView]))
        plt.legend(loc = 0)
        #######################################################################
        plt.figure()                
        plt.plot(t,map(lambda x: float(x), self.SeqDtE[:EndView]), label = 'DtE')
        plt.plot(t,map(lambda x: float(x), self.SeqPD[:EndView]), label = 'Pd')
        plt.plot(t,map(lambda x: float(x), self.SeqPS[:EndView]), label = 'Ps')
        plt.plot(t,map(lambda x,y,z: float(x)+float(y)-float(z), self.SeqDtE[:EndView],self.SeqPD[:EndView],self.SeqPS[:EndView]), label = 'Balance')
        plt.legend(loc = 0)

#######################################################################
#######################################################################

    def PlotStates(self, EndView = 0, fs=48000):
        EndView = EndView if bool(EndView) else self.nt
        nc= np.ceil(np.sqrt(self.nx()))
        nl= np.ceil(self.nx()/float(nc))
        t = np.linspace(0,float((EndView-1))/fs, EndView)
        plt.figure()    
        for n in range(self.nx()):
            plt.subplot(nl,nc,n+1)
            plt.plot(t,map(lambda x: float(x[n]), self.SeqX[:EndView]), label = str(self.x[n]))
            plt.legend(loc = 0)

    def PlotPorts(self, EndView = 0, fs=48000):
        EndView = EndView if bool(EndView) else self.nt
        t = np.linspace(0,float((EndView-1))/fs, EndView)
        for n in range(self.ny()):
            plt.figure()   
            plt.subplot(2,1,1)             
            plt.plot(t,map(lambda x: float(x[n]), self.SeqU[:EndView]), label = str(self.u[n]))
            plt.legend(loc = 0)
            plt.subplot(2,1,2)             
            plt.plot(t,map(lambda x: float(x[n]), self.SeqY[:EndView]), label = str(self.y[n]))
            plt.legend(loc = 0)

#######################################################################
#######################################################################

    def PlotdxHs(self, fs, DebdView = 0., EndView = 0., IndexToPlot = None):
        self.nt = self.SeqX.__len__()
        EndView = EndView if bool(EndView) else self.nt
        t = np.linspace(DebdView/fs,float((EndView-1))/fs, EndView-DebdView)
        #######################################################################
        listPlots = range(self.nx()) if IndexToPlot == None else list(IndexToPlot)
        #######################################################################
        for n in listPlots:
            plt.figure()    
            plt.subplot(2,2,1)
            plt.plot(t,map(lambda x: float(x[n]), self.SeqX[:EndView]), label = str(self.x[n]))
            plt.legend(loc = 0)
            plt.subplot(2,2,3)
            plt.plot(t,map(lambda x: float(x[n]), self.SeqDxH[:EndView]), label = 'H(t)')
            plt.legend(loc = 0)
            plt.subplot(1,2,2)
            plt.plot(map(lambda x: float(x[n]), self.SeqX[:EndView]),map(lambda x: float(x[n]), self.SeqDxH[:EndView]), label = 'H('+ str(self.x[n]) + ')')
            plt.legend(loc = 0)

def Expr2Cpp(expr, dict_args, phs_label, label_func, Out_variables, out="c"):

    arg_labels = dict_args.keys()
    na = arg_labels.__len__()
    
    expr_symbols = sp.Matrix(expr).free_symbols if expr.__len__()>0 else []    
    nexpr = expr.__len__()
    
    str_args = ""    
    for n in range(na):
        str_args += "vector<double>& " + arg_labels[n]
        if n<na-1:
            str_args += ", "    
    str_header =  label_func +"()"

    str_init = ""
    for n in range(na):
        list_args = [e for e in dict_args[arg_labels[n]] ]
        str_args = ""
        naa = list_args.__len__()
        if naa>0:
            for m in range(naa):
                test = set([list_args[m]]).issubset(expr_symbols) if expr.__len__()>0 else False
                if test:
                    if str_args.__len__() == 0:
                        str_args += '    const double '
                    else:
                        str_args += ', '
                    str_args += str(list_args[m]) + " = "+arg_labels[n]+"["+str(m)+"]"
            if str_args.__len__() > 0:
                str_init += str_args +";\n"
    str_init += '    double '
    for n in range(nexpr):
        end_line = ', ' if n<nexpr-1 else ';'
        str_init += 'dummy_float'+str(n)+' = '+str(0) + end_line        
    

    if type(Out_variables)==str:
        label_out = [Out_variables+'['+str(n)+']' for n in range(nexpr)]
    else :
        label_out = Out_variables

    str_out1 = ""
    str_out2 = ""
    
    if out=="c":
        widgets = ['\nWrite '+ label_func, pb.Percentage(), ' ', pb.ETA()]
        pbar = pb.ProgressBar(widgets=widgets, maxval=nexpr).start()
        for n in range(nexpr):
            str_out1 += "    dummy_float"+str(n)+" = "+ccode(expr[n]) +";\n"
            str_out2 += "    "+label_out[n]+" = dummy_float"+str(n)+";\n"
            pbar.update(n+1)
        str_c = "\n"+"void "+phs_label+"::"+str_header + " {\n"+ str_init + "\n" + str_out1+str_out2 + "\n}\n"
        return str_c
    else: 
        str_h = "    "+"void " +str_header + ";\n"
        return str_h



def hfile(phs, NbItNR=100, NumTolNr=1e-12):

    print("\nWrite "+phs.folder+"/" + phs.label.upper() + '.h')

    header_label = phs.label.upper()+"_H"
    str_test = "#ifndef " + header_label + "\n#define " + header_label +"\n\n"
    
    str_includes =  '#include "iostream"\n#include "vector"\n#include "math.h"\n\n'+\
                    "using namespace std;\n\n"

    str_class = "class " + phs.label.upper() + " {\n"

    str_publics_variables = "public:\n\n"
    str_constructor = "\n    "+"// Default Constructor:\n"+"    "+phs.label.upper()+"();\n"    
    str_initx_constructor = "\n    "+"// Constructor Initializing x:\n"+"    "+phs.label.upper()+"(vector<double>&);\n"    
    str_destructor = "\n    "+"// Default Destructor:\n"+"    "+ "~"+phs.label.upper()+"();\n"    

    str_accesors = "\n    // Accesors to private variables\n"
    str_accesors += "    vector<double> get_y() const;\n"
    str_accesors += "    vector<double> get_u() const;\n"
    str_accesors += "    vector<double> get_x() const;\n"
    str_accesors += "    vector<double> get_dx() const;\n"
    str_accesors += "    vector<double> get_dxH() const;\n"
    str_accesors += "    vector<double> get_w() const;\n"
    str_accesors += "    vector<double> get_z() const;\n"
    str_accesors += "    const unsigned int get_nx() const;\n"
    str_accesors += "    const unsigned int get_nxl() const;\n"
    str_accesors += "    const unsigned int get_nxnl() const;\n"
    str_accesors += "    const unsigned int get_nw() const;\n"
    str_accesors += "    const unsigned int get_nwl() const;\n"
    str_accesors += "    const unsigned int get_nwnl() const;\n"
    str_accesors += "    const unsigned int get_ny() const;\n"
    str_accesors += "    const unsigned int get_np() const;\n"

    str_modificators = "\n    // Mutators of private variables\n"
    str_modificators += "    void set_u(vector<double>);\n"
    str_modificators += "    void set_x(vector<double>);\n"
    
    str_phs_functions = "\n    // Port-Hamiltonian runtime functions\n"
    
    dict_args = {"x":phs.x, "dx": phs.dx, "w": phs.w, "u": phs.u, "p": phs.params}
    phs_label = phs.label.upper()

    expr = list(phs.Fl)
    label_variable = ["dxl["+str(n)+"]" for n in range(phs.nxl())]+ ["wl["+str(n)+"]" for n in range(phs.nwl())]
    label_func = "Fl"
    str_phs_functions += Expr2Cpp(expr,dict_args,phs_label, label_func, label_variable, "h")

    if phs.isNL:
        expr = list(sp.Matrix(phs.dxnl()+phs.wnl())+sp.Matrix(phs.Fnl))
        label_variable = ["dxnl["+str(n)+"]" for n in range(phs.nxnl())]+ ["wnl["+str(n)+"]" for n in range(phs.nwnl())]
        label_func = "Fnl"
        str_phs_functions += Expr2Cpp(expr,dict_args,phs_label, label_func, label_variable, "h")
    
        expr = [phs.Fnl_residual]
        label_variable = "residual_NR"
        label_func = "FresidualNR"
        str_phs_functions += Expr2Cpp(expr,dict_args,phs_label, label_func, label_variable, "h").replace('residual_NR[0] ', 'residual_NR ')

    expr = phs.dxH
    label_variable = "dxH"
    label_func = "FdxH"
    str_phs_functions += Expr2Cpp(expr,dict_args,phs_label, label_func, label_variable, "h")

    expr = phs.z
    label_variable = "z"
    label_func = "Fz"
    str_phs_functions += Expr2Cpp(expr,dict_args,phs_label, label_func, label_variable, "h")
            
    expr = phs.Fy
    label_variable = "y"
    label_func = "Fy"
    str_phs_functions += Expr2Cpp(expr,dict_args,phs_label, label_func, label_variable, "h")
    
    str_phs_functions += "    void process(vector<double>&, vector<double>&);\n"
    
    str_privates_variables = "\nprivate:\n\n"

    str_privates_variables += "    // Dimensions of the port-Hamiltonian system\n"
    str_privates_variables += "    "+"const unsigned int nx=" + str(phs.nx()) + ";\n"
    str_privates_variables += "    "+"const unsigned int nxl=" + str(phs.nxl()) + ";\n"
    str_privates_variables += "    "+"const unsigned int nxnl=" + str(phs.nxnl()) + ";\n"
    str_privates_variables += "    "+"const unsigned int nw=" + str(phs.nw()) + ";\n"
    str_privates_variables += "    "+"const unsigned int nwl=" + str(phs.nwl()) + ";\n"
    str_privates_variables += "    "+"const unsigned int nwnl=" + str(phs.nwnl()) + ";\n"
    str_privates_variables += "    "+"const unsigned int ny=" + str(phs.ny()) + ";\n"
    str_privates_variables += "    "+"const unsigned int np=" + str(phs.params.__len__()) + ";\n\n"
    
    str_privates_variables += "    // Port-Hamiltonian system's vectors\n"
    str_privates_variables += "    vector<double> x = vector<double>(" + str(phs.nx()) + ");\n"
    str_privates_variables += "    vector<double> dx = vector<double>(" + str(phs.nx()) + ");\n"
    str_privates_variables += "    vector<double> dxH = vector<double>(" + str(phs.nx()) + ");\n"
    str_privates_variables += "    vector<double> w = vector<double>(" + str(phs.nw()) + ");\n"
    str_privates_variables += "    vector<double> z = vector<double>(" + str(phs.nw()) + ");\n"
    str_privates_variables += "    vector<double> u = vector<double>(" + str(phs.ny()) + ");\n"
    str_privates_variables += "    vector<double> y = vector<double>(" + str(phs.ny()) + ");\n"
    str_privates_variables += "    vector<double> p = vector<double>(" + str(phs.params.__len__()) + ");\n\n"
    
    str_privates_variables += "    // Pointers to the linear part of each vector\n"
    str_privates_variables += "    double * xl = "+"& x[0];\n"
    str_privates_variables += "    double * dxl = "+"& dx[0];\n"
    str_privates_variables += "    double * dxHl = "+"& dxH[0];\n"
    str_privates_variables += "    double * wl = "+"& w[0];\n"
    str_privates_variables += "    double * zl = "+"& z[0];\n\n"

    str_privates_variables += "    // Pointers to the nonlinear part of each vector\n"
    str_privates_variables += "    double * xnl = "+"& x[" + str(phs.nxl())+ "];\n"
    str_privates_variables += "    double * dxnl = "+"& dx[" + str(phs.nxl())+ "];\n"
    str_privates_variables += "    double * dxHnl = "+"& dxH[" + str(phs.nxl())+ "];\n"
    str_privates_variables += "    double * wnl = "+"& w[" + str(phs.nwl())+ "];\n"
    str_privates_variables += "    double * znl = "+"& z[" + str(phs.nwl())+ "];\n\n"

    str_privates_variables += "    // Variables for NR iterations\n"
    str_privates_variables += "    double NbItNR = " + str(NbItNR) + ";\n"
    str_privates_variables += "    double residual_NR = 0;\n"
    str_privates_variables += "    double it = 0;\n"
    str_privates_variables += "    double eps = "+ str(NumTolNr) + ";\n"

    str_outro = "};\n#endif /* " + header_label + " */"
    str_out = str_test + str_includes + str_class + str_publics_variables  +\
            str_constructor +str_initx_constructor+str_destructor+ str_accesors +\
            str_modificators +str_phs_functions+ str_privates_variables + str_outro
    h_file = open(phs.folder+"/"+phs.label+".h", 'w')
    h_file.write(str_out)
    h_file.close()
    return str_out
    
def cfile(phs):

    print("\nWrite "+phs.folder+"/" + phs.label.upper() + '.c')
    time.sleep(0.1)
    
    class_ref = phs.label.upper()+"::"
    str_includes =  '#include "'+phs.folder+"/" + phs.label.upper() + '.h"\n\n'

    str_variables = ""    
    str_constructor = "\n// Default Constructor:\n"+class_ref+phs.label.upper()+"() {\n"+str_variables+"}\n"   
    str_initx_constructor = "\n// Constructor Initializing x:\n"+class_ref+\
                            phs.label.upper()+"(vector<double>& x0) {\n"  +\
                            str_variables+'    if (x.size() == x0.size()) {\n        x = x0;\n    }\n    else {\n        cerr << "Size of x0 does not match size of x" << endl;\n        exit(1);\n    }\n}\n' 
                            
    str_destructor = "\n// Default Destructor:\n"+class_ref+ "~"+phs.label.upper()+"() {\n\n}\n"    
    
    str_accesors = "\n// Accesors to private variables\n\n"

    str_accesors += "const unsigned int "+class_ref+"get_nx() const {\n    return nx;\n}\n"
    str_accesors += "const unsigned int "+class_ref+"get_nxl() const {\n    return nxl;\n}\n"
    str_accesors += "const unsigned int "+class_ref+"get_nxnl() const {\n    return nxnl;\n}\n\n"

    str_accesors += "const unsigned int "+class_ref+"get_nw() const {\n    return nw;\n}\n"
    str_accesors += "const unsigned int "+class_ref+"get_nwl() const {\n    return nwl;\n}\n"
    str_accesors += "const unsigned int "+class_ref+"get_nwnl() const {\n    return nwnl;\n}\n\n"

    str_accesors += "const unsigned int "+class_ref+"get_ny() const {\n    return ny;\n}\n\n"

    str_accesors += "const unsigned int "+class_ref+"get_np() const {\n    return np;\n}\n\n"

    str_accesors += "vector<double> "+class_ref+"get_u() const {\n    return u;\n}\n"
    str_accesors += "vector<double> "+class_ref+"get_y() const {\n    return y;\n}\n\n"

    str_accesors += "vector<double> "+class_ref+"get_x() const {\n    return x;\n}\n"
    str_accesors += "vector<double> "+class_ref+"get_dx() const {\n    return dx;\n}\n"
    str_accesors += "vector<double> "+class_ref+"get_dxH() const {\n    return dxH;\n}\n\n"

    str_accesors += "vector<double> "+class_ref+"get_w() const {\n    return w;\n}\n"
    str_accesors += "vector<double> "+class_ref+"get_z() const {\n    return z;\n}\n"

    str_modificators = "\n// Mutators of private variables\n"
    str_modificators += "void "+class_ref+"set_u(vector<double> v) {\n    u = v; \n}\n"
    str_modificators += "void "+class_ref+"set_x(vector<double> v) {\n    x = v; \n}\n"
    
    str_phs_functions = "\n// Port-Hamiltonian runtime functions\n"
    
    dict_args = {"x":phs.x, "dx": phs.dx, "w": phs.w, "u": phs.u, "p": phs.params}
    phs_label = phs.label.upper()

    expr = list(phs.Fl)
    label_variable = ["dx["+str(n)+"]" for n in range(phs.nxl())]+ ["w["+str(n)+"]" for n in range(phs.nwl())]
    label_func = "Fl"
    str_phs_functions += Expr2Cpp(expr,dict_args,phs_label, label_func, label_variable, "c")

    if phs.isNL:
        expr = list(sp.Matrix(phs.dxnl()+phs.wnl())+sp.Matrix(phs.Fnl))
        label_variable = ["dx["+str(phs.nxl()+n)+"]" for n in range(phs.nxnl())]+ ["wnl["+str(phs.nwl()+n)+"]" for n in range(phs.nwnl())]
        label_func = "Fnl"
        str_phs_functions += Expr2Cpp(expr,dict_args,phs_label, label_func, label_variable, "c")
        
        expr = [phs.Fnl_residual]
        label_variable = "residual_NR"
        label_func = "FresidualNR"
        str_phs_functions += Expr2Cpp(expr,dict_args,phs_label, label_func, label_variable, "c").replace('residual_NR[0] ', 'residual_NR ')

    expr = phs.dxH
    label_variable = "dxH"
    label_func = "FdxH"
    str_phs_functions += Expr2Cpp(expr,dict_args,phs_label, label_func, label_variable, "c")

    expr = phs.z
    label_variable = "z"
    label_func = "Fz"
    str_phs_functions += Expr2Cpp(expr,dict_args,phs_label, label_func, label_variable, "c")
            
    expr = phs.Fy
    label_variable = "y"
    label_func = "Fy"
    str_phs_functions += Expr2Cpp(expr,dict_args,phs_label, label_func, label_variable, "c")

    str_NR = \
    """
                // Newton-Raphson iteration
                it = 0;
                residual_NR = 1;
                while ((it<NbItNR) & (residual_NR>eps)) {
                    Fnl(); 
                    FresidualNR();
                    it++;
                }
    """ if (phs.isNL) else ""
    str_NR = "void "+phs_label+"::process(vector<double>& In, vector<double>& parameters) {\n "+\
    """
            
            u = In;
            p = parameters;
    """ + str_NR + \
    """
            Fl();
            FdxH();
            Fz();
            Fy();
            for (unsigned int i=0; i<get_nx(); i++ ) {
                x[i] += dx[i];
            }
    """ +"}\n"

    str_c =  str_includes + str_constructor+str_NR + str_initx_constructor+str_destructor+\
                str_accesors+str_modificators+str_phs_functions

    c_file = open(phs.folder+"/"+phs.label+".cpp", 'w')
    c_file.write(str_c)
    c_file.close()
    return str_c

def MainCpp(phs):
    str_main = """
//
//  main.cpp
//  phs
//
//  Created by Antoine Falaize on 21/09/2015.
//  Copyright (c) 2015 Antoine Falaize. All rights reserved.
//

#include <iostream>
"""+ '#include "'+phs.folder+"/" + phs.label.upper() + '.h"\n\n'+\
"""
#include "vector"

#include <fstream>
#include <string>

int main() {

    ifstream x0File;
    x0File.open("""+ '"'+phs.folder+"/"+ """x0.txt");

    if (x0File.fail()) {
        cerr << "Failed opening x0 file" << endl;
        exit(1);
    }

    ifstream uFile;
    uFile.open("""+ '"'+phs.folder+"/"+ """u.txt");

    if (uFile.fail()) {
        cerr << "Failed opening input file" << endl;
        exit(1);
    }

    ifstream pFile;
    pFile.open("""+'"'+ phs.folder+"/"+ """p.txt");

    if (pFile.fail()) {
        cerr << "Failed opening parameters file" << endl;
        exit(1);
    }

    unsigned int nt = 0;

    vector<double> xVector("""+ str(phs.nx()) +""");
    vector<double> x0Vector("""+ str(phs.nx()) +""");
    vector<double> dxHVector("""+ str(phs.nx()) +""");
    vector<double> dxVector("""+ str(phs.nx()) +""");
    vector<double> wVector("""+ str(phs.nw()) +""");
    vector<double> zVector("""+ str(phs.nw()) +""");
    vector<double> uVector("""+ str(phs.ny()) +""");
    vector<double> yVector("""+ str(phs.ny()) +""");
    vector<double> pVector("""+ str(phs.params.__len__()) +""");

    typedef std::vector< std::vector<double> > matrix;

    matrix matU(0, vector<double>("""+ str(phs.ny()) +"""));
    matrix matP(0, vector<double>("""+ str(phs.np()) +"""));

    for (unsigned int i=0; i<"""+ str(+phs.nx()) +"""; i++) {
        x0File >> x0Vector[i];
    }

"""+ '    ' + phs.label.upper() +' sys(x0Vector);'+\
"""
    
    
    // Get input data
    cout <<"Reading input..." << endl;
    while (!uFile.eof()) {
    
        for (unsigned int i=0; i<sys.get_ny(); i++) {
            uFile >> uVector[i];
        }

        // Get parameters data
        for (unsigned int i=0; i<sys.get_np(); i++) {
            pFile >> pVector[i];
        }
        matU.push_back(uVector);
        matP.push_back(pVector);

        nt = unsigned(int(matU.size()));
    }

    uFile.close();
    pFile.close();

    nt -= 1;
    
    matrix matX(nt, vector<double>("""+ str(phs.nx()) +"""));
    matrix matdX(nt, vector<double>("""+ str(phs.nx()) +"""));
    matrix matdxH(nt, vector<double>("""+ str(phs.nx()) +"""));
    matrix matW(nt, vector<double>("""+ str(phs.nw()) +"""));
    matrix matZ(nt, vector<double>("""+ str(phs.nw()) +"""));
    matrix matY(nt, vector<double>("""+ str(phs.ny()) +"""));

    // Compute
    cout <<"Process simulation..." << endl;
    for (unsigned int n=0; n<nt; n++) {
        
        //cout <<"Simulation " <<  int(100*float(n)/float(nt)) << "%" << endl;
        
        uVector = matU[n];
        pVector = matP[n];

        // Get x before process update
        xVector = sys.get_x();

        // Process update
        sys.process(uVector, pVector);

        // Get quantities
        dxVector = sys.get_dx();
        dxHVector = sys.get_dxH();
        wVector = sys.get_w();
        zVector = sys.get_z();
        yVector = sys.get_y();

        matX[n] = xVector;
        matdX[n] = dxVector;
        matdxH[n] = dxHVector;
        matW[n] = wVector;
        matZ[n] = zVector;
        matY[n] = yVector;
    }

    ofstream xFile;
    xFile.open("""+'"'+ phs.folder+"/"+ """x.txt");
    ofstream dxHFile;
    dxHFile.open("""+'"'+ phs.folder+"/"+ """dxH.txt");
    ofstream dxFile;
    dxFile.open("""+'"'+ phs.folder+"/"+ """dx.txt");

    ofstream wFile;
    wFile.open("""+'"'+ phs.folder+"/"+ """w.txt");
    ofstream zFile;
    zFile.open("""+'"'+ phs.folder+"/"+ """z.txt");

    ofstream yFile;
    yFile.open("""+'"'+ phs.folder+"/"+ """y.txt");

    // Save
    cout << "Write output..."<< endl;
    for (unsigned int n=0; n<nt; n++) {

        // cout << "Writing Data " << int(100.0*float(n)/nt) << endl;
        xVector = matX[n];
        dxVector = matdX[n];
        dxHVector = matdxH[n];

        wVector = matW[n];
        zVector = matZ[n];

        yVector = matY[n];
        
        // Save quantities
        for (unsigned int i=0; i<sys.get_nx(); i++) {
            xFile << xVector[i] << " ";
            dxFile << dxVector[i] << " ";
            dxHFile << dxHVector[i] << " ";
        }

        for (unsigned int i=0; i<sys.get_nw(); i++) {
            wFile << wVector[i] << " ";
            zFile << zVector[i] << " ";
        }

        for (unsigned int i=0; i<sys.get_ny(); i++) {
            yFile << yVector[i] << " ";
        }

        xFile << endl;
        dxFile << endl;
        dxHFile << endl;

        wFile << endl;
        zFile << endl;    

        yFile << endl;
        
    }

    xFile.close();
    dxFile.close();
    dxHFile.close();
    wFile.close();
    zFile.close();
    yFile.close();

    cout << endl;
    cout << "Data written at" << endl;
    cout << endl;
    cout << """+'"'+ phs.folder+"/"+'"' +"""<< endl;
    cout << endl;
    
    return 0;
}
"""
    main_file = open(phs.folder+"/main.cpp", 'w')
    main_file.write(str_main)
    main_file.close()
    return str_main