import numpy as np
import scipy.signal as sig
import pandas as pd

from sympy import *
init_printing()
from IPython.display import display, Math, Latex
 
from myhdl import *
from myhdlpeek import Peeker

import matplotlib.pyplot as plt
%matplotlib inline

"""
Explanation: \title{Cascaded Integrator Comb (CIC) Filter in myHDL}
\author{Steven K Armour}
\maketitle
Python Libraries Utilized
End of explanation
"""


n, M=symbols('n, M')
y=Function('y')(n)
x=Function('x')

Comb1Eq=Eq(y, x(n)-x(n-1)); Comb1Eq

"""
Explanation: Acknowledgments
The orgianl CIC filter written in myHDL was done by Christopher Felton
(myHDL.old version here)
Author of myHDL Jan Decaluwe and the author of the myHDL Peeker XESS Corp.
Refrances
CIC Filter Backround and Pro/Cons of use
CIC Comb (dervitive) Unit
1st Order Comb Unit
Sympy Anyslsis
One of the two fundumental stages of the CIC filter is the Derivative stage also called a Comb. The lowest order Comb filter is governed by the following equation
End of explanation
"""


z=symbols('z')
Y=Function('Y')(z); X=Function('X')(z)
Comb1ZEq=Eq(Y, (1-z**-1)*X); Comb1ZEq

"""
Explanation: where by applying the "Z" Transform to 1st order Comb equation we get
End of explanation
"""


H=Function('H')(z)
Comb1TFEq=Eq(H, solve(Comb1ZEq, Y)[0]/X); Comb1TFEq

"""
Explanation: Which results in the following Transfer Function and discrate inpulse responce
End of explanation
"""


TFnum, TFden=fraction(Comb1TFEq.rhs); TFnum, TFden
TFZeros=solve(TFnum, z); TFPoles=solve(TFden)
TFPZStatment=f"""{H} has zeros from {TFnum} at {TFZeros}
and poles from {TFden} at {TFPoles}"""
print(TFPZStatment)

"""
Explanation: The Transfer function has the following zeros and poles
End of explanation
"""


TFCoefN=lambda NumDen: [float(i) for i in Poly(NumDen, z).coeffs()]
TFnumC=TFCoefN(TFnum); TFdenC=TFCoefN(TFden)
TFnumC, TFdenC

Comb1Sci=sig.TransferFunction(TFnumC, [1, 0], dt=1/4.8e3)
Comb1Sci

w, mag, phase = sig.dbode(Comb1Sci )
fig, ax=plt.subplots(nrows=2, ncols=1, sharex=True)
plt.title('Bode Plot of 1st order Comb Section')
ax[0].semilogx(w, mag, label='Mag')
ax[0].set_ylabel('Mag [dB]')
ax[1].semilogx(w, phase, label='Phase')
ax[1].set_ylabel('Phase[deg]')
ax[1].set_xlabel(r'$\omega[rad/dt]$')
None


plt.title('Impulse Respince of 1st Order Comb Filter')
tout, yout=sig.dimpulse(Comb1Sci, n=100)
plt.plot(tout, yout[0])
plt.xlabel('time[sec]'); plt.ylabel('Amp')
plt.ticklabel_format(style='sci', axis='x', scilimits=(0,0))

plt.title('Step Respince of 1st Order Comb Filter')
tout, yout=sig.dstep(Comb1Sci, n=100)
plt.plot(tout, yout[0])
plt.xlabel('time[sec]'); plt.ylabel('Amp')
plt.ticklabel_format(style='sci', axis='x', scilimits=(0,0))

"""
Explanation: scipy anyslsis
using the exspersion for the numurator and denomnator there coefecnts can be build off thusly and read into scipy's signal module to numerical inspect the resulting transfer function thusly where we will use a sample time
End of explanation
"""


M=symbols('M')
CombMEq=Eq(y, x(n)-x(n-M)); CombMEq

CombMZEq=Eq(Y, (1-z**-M)*X); CombMZEq

H=Function('H')(z)
CombMTFEq=Eq(H, simplify(solve(CombMZEq, Y)[0]/X)); CombMTFEq
CombMTFEq=Eq(H, expand(CombMTFEq.rhs*z**M)/z**M); CombMTFEq

sups={M:3}
CombMTFEq=CombMTFEq.subs(sups); CombMTFEq


TFnum, TFden=fraction(CombMTFEq.rhs); TFnum, TFden
TFZeros=solve(TFnum, z); TFPoles=solve(TFden)
TFPZStatment=f"""{H} has zeros from {TFnum} at {TFZeros}
and poles from {TFden} at {TFPoles}"""
print(TFPZStatment)

TFCoefN=lambda NumDen: [float(i) for i in Poly(NumDen, z).coeffs()]
TFnumC=TFCoefN(TFnum); TFdenC=TFCoefN(TFden)
for i in range(sups[M]-1):
    TFnumC.insert(1, 0)

for i in range(sups[M]):
    TFdenC.insert(1, 0)
TFnumC, TFdenC

CombMSci=sig.TransferFunction(TFnumC, TFdenC, dt=1/4.8e3)
CombMSci

w, mag, phase = sig.dbode(CombMSci )
fig, ax=plt.subplots(nrows=2, ncols=1, sharex=True)
plt.title(f'Bode Plot of {sups[M]} order Comb Section')
ax[0].semilogx(w, mag, label='Mag')
ax[0].set_ylabel('Mag [dB]')
ax[1].semilogx(w, phase, label='Phase')
ax[1].set_ylabel('Phase[deg]')
ax[1].set_xlabel(r'$\omega[rad/dt]$')
None

plt.title(f'Impulse Respince of {sups[M]} order Comb Section')
tout, yout=sig.dimpulse(CombMSci, n=100)
plt.plot(tout, yout[0])
plt.xlabel('time[sec]'); plt.ylabel('Amp')
plt.ticklabel_format(style='sci', axis='x', scilimits=(0,0))

plt.title(f'Step Respince of {sups[M]} order Comb Section')
tout, yout=sig.dstep(CombMSci, n=100)
plt.plot(tout, yout[0])
plt.xlabel('time[sec]'); plt.ylabel('Amp')
plt.ticklabel_format(style='sci', axis='x', scilimits=(0,0))

"""
Explanation: M-th Order Comb Unit
End of explanation
"""


TestValues=pd.read_pickle('SinGen16.pkl')
plt.stem(TestValues['Time[s]'], TestValues['GenValue'])
plt.plot(TestValues['Time[s]'], TestValues['GenValue'])
plt.ticklabel_format(style='sci', axis='x', scilimits=(0,0))


def CombDelay(x_in, y_out, DataValIn, clk ):
    """
    Modfied D flip-flop repersenting a delay of z^-1
    Ports:
    ------
    x_in:    2's compliment input
    y_out:    2's compliment output
    DataValIn:  data valid input, flow control signal for the different rates
    clk:  System clock, must be equal to or greater than the max
          sample rate after interpolation or before decimation
    """
    @always(clk.posedge)
    def logic():
        if DataValIn:
            y_out.next=x_in
    return logic

"""
Explanation: myHDL Implimintation of M-th Order Comb Section
$z^{-1}$ as a delay
End of explanation
"""


BitWidth=16
L=2**(BitWidth)
maxminV =2*L 
Peeker.clear()

x_in=Signal(intbv(0, min=-L, max=L)); Peeker(x_in, 'x_in')

y_out=Signal(intbv(0, min=-maxminV, max=maxminV)); Peeker(y_out, 'y_out')
CombDelayTracker=[]

DataValIn=Signal(bool(0)); Peeker(DataValIn, 'DataValIn')
clk=Signal(bool(0)); Peeker(clk, 'clk')

DUT=CombDelay(x_in=x_in, y_out=y_out, DataValIn=DataValIn, clk=clk)


def CombDelay_TB():
    TestValueGen=TestValues['GenValue'].iteritems()
    
    @always(delay(1))    
    def clkgen():
        clk.next = not clk
    
    @instance
    def stimules():
        #test the DataValIn responce
        for step, val in TestValueGen:
            x_in.next=int(val)
            if step<12:
                DataValIn.next=False
            else:
                DataValIn.next=1
            CombDelayTracker.append(int(y_out))
            
            yield clk.negedge
        
        raise StopSimulation
        
    return instances()
        

N=TestValues.shape[0]
sim = Simulation(DUT, CombDelay_TB(), *Peeker.instances()).run()
Peeker.to_wavedrom(start_time=40, stop_time=60, tock=True)

plt.stem(TestValues['Time[s]'], TestValues['GenValue']-np.array(CombDelayTracker))
plt.ticklabel_format(style='sci', axis='x', scilimits=(0,0))


"""
Explanation: Using the output sin genratr values with 16 bit persisoin we optain the following test data
End of explanation
"""


def CombSection(x_in, y_out, DataValIn, DataValOut, clk, rst, MthOrderParm=1):
    """
     Comb (difference) section of CIC with parameterizable to Mth Order
    
    Parameters:
    -----------
    MthOrderParm: Delay Order of the Comb section
    
    Ports:
    ------
    x_in:    2's compliment input
    y_out:    2's compliment output
    DataValIn:  data valid input, flow control signal for the different rates
    clk:  System clock, must be equal to or greater than the max
          sample rate after interpolation or before decimation
    rst:  System reset
    """
    #quntiztion
    Q = len(x_in)-1
    #word lengths
    L = 2**Q
    
    #create the dealy structer
    #Comb Delay Instansis
    Delay_i = [None for i in range(MthOrderParm)]
    #Comb Delay Instansis Interconects
    Delay_iX = [Signal(intbv(0, min=-L, max=L)) for i in range(MthOrderParm)]
    
    #output of delay block = to x[n-M]
    subx=Delay_iX[MthOrderParm-1]
    
    for i in range(MthOrderParm):
        if i ==0:
            Delay_i[i]=CombDelay(x_in=x_in, y_out=Delay_iX[i], DataValIn=DataValIn, clk=clk)
        else:
            Delay_i[i]=CombDelay(x_in=Delay_iX[i-1], y_out=Delay_iX[i], DataValIn=DataValIn, clk=clk)

    @always(clk.posedge)
    def logic():
        if rst:
            y_out.next=0
        else:
            if DataValIn:
                y_out.next=x_in-subx
                DataValOut.next=True
            else:
                DataValOut.next=False
    return instances()


"""
Explanation: Comb Section
End of explanation
"""


BitWidth=16
L=2**(BitWidth)
maxminV =2*L 
Peeker.clear()

x_in=Signal(intbv(0, min=-L, max=L)); Peeker(x_in, 'x_in')

y_out=Signal(intbv(0, min=-maxminV, max=maxminV)); Peeker(y_out, 'y_out')
CombTracker=[]

DataValIn=Signal(bool(0)); Peeker(DataValIn, 'DataValIn')
DataValOut=Signal(bool(0)); Peeker(DataValOut, 'DataValOut')

clk=Signal(bool(0)); Peeker(clk, 'clk')
rst=Signal(bool(0)); Peeker(rst, 'rst')


DUT=CombSection(x_in=x_in, y_out=y_out, 
                DataValIn=DataValIn, DataValOut=DataValOut, 
                clk=clk, rst=rst, 
                MthOrderParm=1)



def CombSection_TB():
    TestValueGen=TestValues['GenValue'].iteritems()
    
    @always(delay(1))    
    def clkgen():
        clk.next = not clk
    
    @instance
    def stimules():
        #test the DataValIn responce
        for step, val in TestValueGen:
            x_in.next=int(val)
            if step<12:
                DataValIn.next=False
            
            elif step==12:
                DataValIn.next=True
                
            elif step==100:
                rst.next=True
                DataValIn.next=False
            
            elif step==101:
                rst.next=False
                DataValIn.next=False
            
            elif step==112:
                DataValIn.next=False
                
            else:
                DataValIn.next=True
            CombTracker.append(int(y_out))
            
            yield clk.negedge
        
        raise StopSimulation
        
    return instances()
        

N=TestValues.shape[0]
sim = Simulation(DUT, CombSection_TB(), *Peeker.instances()).run()
Peeker.to_wavedrom(start_time=40, stop_time=60, tock=True)

plt.stem(TestValues['Time[s]'], TestValues['GenValue']-np.array(CombTracker))
plt.ticklabel_format(style='sci', axis='x', scilimits=(0,0))


BitWidth=16
L=2**(BitWidth)
maxminV =2*L 
Peeker.clear()

x_in=Signal(intbv(0, min=-L, max=L)); Peeker(x_in, 'x_in')

y_out=Signal(intbv(0, min=-maxminV, max=maxminV)); Peeker(y_out, 'y_out')
CombTracker=[]

DataValIn=Signal(bool(0)); Peeker(DataValIn, 'DataValIn')
DataValOut=Signal(bool(0)); Peeker(DataValOut, 'DataValOut')

clk=Signal(bool(0)); Peeker(clk, 'clk')
rst=Signal(bool(0)); Peeker(rst, 'rst')


DUT=CombSection(x_in=x_in, y_out=y_out, 
                DataValIn=DataValIn, DataValOut=DataValOut, 
                clk=clk, rst=rst, 
                MthOrderParm=3)

N=TestValues.shape[0]
sim = Simulation(DUT, CombSection_TB(), *Peeker.instances()).run()
Peeker.to_wavedrom(start_time=40, stop_time=60, tock=True)

plt.stem(TestValues['Time[s]'], TestValues['GenValue']-np.array(CombTracker))
plt.ticklabel_format(style='sci', axis='x', scilimits=(0,0))


"""
Explanation: Comb Test
End of explanation
"""


y=Function('y')
Accum1Eq=Eq(y(n), x(n)+y(n-1)); Accum1Eq

Accum1ZEq=Eq(Y, X+z**-1 *Y); Accum1ZEq

Accum1TFEq=Eq(H, solve(Accum1ZEq, Y)[0]/X); Accum1TFEq

TFnum, TFden=fraction(Accum1TFEq.rhs); TFnum, TFden
TFZeros=solve(TFnum, z); TFPoles=solve(TFden)
TFPZStatment=f"""{H} has zeros from {TFnum} at {TFZeros}
and poles from {TFden} at {TFPoles}"""
print(TFPZStatment)

sups[M]=0
TFCoefN=lambda NumDen: [float(i) for i in Poly(NumDen, z).coeffs()]
TFnumC=TFCoefN(TFnum); TFdenC=TFCoefN(TFden)
for i in range(sups[M]-1):
    TFnumC.insert(1, 0)

for i in range(sups[M]):
    TFdenC.insert(1, 0)
TFnumC, TFdenC

AccumSci=sig.TransferFunction(TFnumC, TFdenC, dt=1/4.8e3)
AccumSci

w, mag, phase = sig.dbode(AccumSci )
fig, ax=plt.subplots(nrows=2, ncols=1, sharex=True)
plt.title(f'Bode Plot of Accum Section')
ax[0].semilogx(w, mag, label='Mag')
ax[0].set_ylabel('Mag [dB]')
ax[1].semilogx(w, phase, label='Phase')
ax[1].set_ylabel('Phase[deg]')
ax[1].set_xlabel(r'$\omega[rad/dt]$')

plt.title(f'Impulse Respince of Accum Section')
tout, yout=sig.dimpulse(AccumSci, n=100)
plt.plot(tout, yout[0])
plt.xlabel('time[sec]'); plt.ylabel('Amp')
plt.ticklabel_format(style='sci', axis='x', scilimits=(0,0))

plt.title(f'Step Respince of Accum Section')
tout, yout=sig.dstep(AccumSci, n=100)
plt.plot(tout, yout[0])
plt.xlabel('time[sec]'); plt.ylabel('Amp')
plt.ticklabel_format(style='sci', axis='x', scilimits=(0,0))

def AccumSection(x_in, y_out, DataValIn, DataValOut, clk, rst):
    """
    Simple Integrator    
    
    
    Ports:
    ------
    x_in:    2's compliment input
    y_out:    2's compliment output
    DataValIn:  data valid input, flow control signal for the different rates
    clk:  System clock, must be equal to or greater than the max
          sample rate after interpolation or before decimation
    rst:  System reset
    """
    
    @always(clk.posedge)
    def logic():
        if rst:
            y_out.next=0
        else:                
            test=0

            if DataValIn and (test<y_out.max and test>y_out.min):
                y_out.next=y_out+x_in
                DataValOut.next=True
            else:
                DataValOut.next=False
    
    return instances()
            
    

BitWidth=16
L=2**(BitWidth)
maxminV =4*L 
Peeker.clear()

x_in=Signal(intbv(0, min=-L, max=L)); Peeker(x_in, 'x_in')

y_out=Signal(intbv(0, min=-maxminV, max=maxminV)); Peeker(y_out, 'y_out')
AccumTracker=[]

DataValIn=Signal(bool(0)); Peeker(DataValIn, 'DataValIn')
DataValOut=Signal(bool(0)); Peeker(DataValOut, 'DataValOut')

clk=Signal(bool(0)); Peeker(clk, 'clk')
rst=Signal(bool(0)); Peeker(rst, 'rst')


DUT=AccumSection(x_in=x_in, y_out=y_out, 
                DataValIn=DataValIn, DataValOut=DataValOut, 
                clk=clk, rst=rst)



def AccumSection_TB():
    TestValueGen=TestValues['GenValue'].iteritems()
    
    @always(delay(1))    
    def clkgen():
        clk.next = not clk
    
    @instance
    def stimules():
        #test the DataValIn responce
        for step, val in TestValueGen:
            x_in.next=int(val)
            if step<12:
                DataValIn.next=False
            
            elif step==12:
                DataValIn.next=True
                
            elif step==100:
                rst.next=True
                DataValIn.next=False
            
            elif step==101:
                rst.next=False
                DataValIn.next=False
            
            elif step==112:
                DataValIn.next=False
                
            else:
                DataValIn.next=True
            AccumTracker.append(int(y_out))
            
        
            
            yield clk.negedge
        
        raise StopSimulation
        
    return instances()
        

N=TestValues.shape[0]
sim = Simulation(DUT, AccumSection_TB(), *Peeker.instances()).run()
Peeker.to_wavedrom(start_time=40, stop_time=60, tock=True)

plt.stem(TestValues['Time[s]'], TestValues['GenValue']-np.array(AccumTracker))
plt.ticklabel_format(style='sci', axis='x', scilimits=(0,0))


"""
Explanation: CIC Accumultor (Integrator) Unit
Sympy anylsis
End of explanation
"""


def PassThroughSection(x_in, y_out, DataValIn, DataValOut):
    """
    Place holder insted of interpoplation or decimation 
    
    Ports:
    ------
    x_in:    2's compliment input
    y_out:    2's compliment output
    DataValIn:  data valid input, flow control signal for the different rates
    """
    
    @always_comb
    def logic():
        y_out.next=x_in
        DataValOut.next=DataValIn
    return logic

BitWidth=16
L=2**(BitWidth)
maxminV =4*L 
Peeker.clear()

x_in=Signal(intbv(0, min=-L, max=L)); Peeker(x_in, 'x_in')

y_out=Signal(intbv(0, min=-maxminV, max=maxminV)); Peeker(y_out, 'y_out')
PassThroughTracker=[]

DataValIn=Signal(bool(0)); Peeker(DataValIn, 'DataValIn')
DataValOut=Signal(bool(0)); Peeker(DataValOut, 'DataValOut')

clk=Signal(bool(0)); Peeker(clk, 'clk')
rst=Signal(bool(0)); Peeker(rst, 'rst')


DUT=PassThroughSection(x_in=x_in, y_out=y_out, 
                       DataValIn=DataValIn, DataValOut=DataValOut)




def PassThroughSection_TB():
    TestValueGen=TestValues['GenValue'].iteritems()
    
    @always(delay(1))    
    def clkgen():
        clk.next = not clk
    
    @instance
    def stimules():
        #test the DataValIn responce
        for step, val in TestValueGen:
            x_in.next=int(val)
            if step<12:
                DataValIn.next=False
            
            elif step==12:
                DataValIn.next=True
                
           
            PassThroughTracker.append(int(y_out))
        
            yield clk.negedge
        
        raise StopSimulation
        
    return instances()
        

N=TestValues.shape[0]
sim = Simulation(DUT, PassThroughSection_TB(), *Peeker.instances()).run()
Peeker.to_wavedrom(start_time=40, stop_time=60, tock=True)

plt.stem(TestValues['Time[s]'], TestValues['GenValue']-np.array(PassThroughTracker))
plt.ticklabel_format(style='sci', axis='x', scilimits=(0,0))


"""
Explanation: CIC Filter Additial Segments
End of explanation
"""


