import numpy as np
import control
from scipy import signal
from scipy import linalg
from itertools import zip_longest
import matplotlib.pyplot as plt
inv = np.linalg.inv
pi = np.pi
sin = np.sin
cos = np.cos
sqrt = np.sqrt
exp = np.exp
atan2 = np.arctan2
log10 = np.log10

s = control.TransferFunction.s

def deconvolve(N,D,i):
    f,R = signal.deconvolve(N,D)
    print("deconvolve (im ersten Schritt N/D sonst R/D):")
    print("f"+i+":"+str(f))
    print("R:"+str(R))
    return(f,R)

def convolve(R):
    R = signal.convolve(R,[1, 0])
    print("convolve with z^(-1): R=R*z^(-1):"+str(R)+"\n")
    return(R)

def print_det(M):
    print("Determinante: "+str(np.around(np.linalg.det(M))))

def print_rank(M):
    print("Rang: "+str(np.linalg.matrix_rank(M)))
    
def print_eig(M):
    print("Eigenwerte:")
    w,v = np.linalg.eig(M)
    for i in w:
        print(np.around(i))
        
def matmul_loop(mats):
    temp = mats[0]
    for i in range(1,len(mats)):
        temp = np.matmul(temp,mats[i])
    return temp

#Benutze eigene Funktion statt control.bode(), da control.bode() oft unerwünscht arrays ausgibt:
def plt_bode(G):
    print("Bode-Diagramm:")
    temp = signal.lti(G.num[0][0],G.den[0][0])
    w, mag, phase = temp.bode()
    plt.plot(w,mag,'blue')
    plt.xscale("log")
    plt.grid()
    plt.legend(['Amplitude mit 20*log10(...)'])
    plt.show()

    plt.plot(w,phase,'orange')
    plt.xscale("log")
    plt.grid()
    plt.legend(['Phase in Grad'])
    plt.show()

#Benutze eigene Funktion um Skalierung an die Ausgabe in Matlab anzupassen:
def plt_nyquist(G,scale_x,scale_y,x0,x1):
    plt.axhline(y=0, color='lightgrey')
    plt.axvline(x=0, color='lightgrey')
    
    control.nyquist(G)
    plt.grid(True)
    plt.title('Nyquist-Diagramm von:\n'+str(G))
    
    xmin, xmax = plt.xlim()
    ymin, ymax = plt.ylim()
    plt.xlim(xmin * scale_x + x0, xmax * scale_x  +x1)
    plt.ylim(ymin * scale_y, ymax * scale_y)
    
    plt.xlabel('Re(s)')
    plt.ylabel('Im(s)')
    plt.show()

#Benutze eigene Funktion um aussagekräftige Ausgabe zu erhalten:
def margin(G):
    gm, pm, wg, wp = control.margin(G)
    if str(pm) == "inf":
        print("keinen Phasenrand gefunden")
    else:
        print("Phasenrand: "+str(np.around(pm,3))+"° bei Frequenz: "+str(np.around(wp,3))+"rad/s")
    if str(gm) == "inf":
        print("keine Amplitudenreserve gefunden")
    else:
        print("Amplitudenreserve: "+str(np.around(gm,3))+" bei Frequenz: "+str(np.around(wg,3))+"rad/s")

import sys
sys.path.append('Pysim')
from Schema import Schema
from Block import (Constant, StateSpace, Gain, StepSource,
Division, Product, Saturation, PI,PID, Sum, SquareSignal,
SinusoidalSignalSource, Integrator, Not, TransferFunction)
from DTBlock import ZOH, DTStateSpace, FIR, DTIntegral, DTDelay, IIR, DTTransferFunction

import ipywidgets as widgets
from ipywidgets import interact