import matplotlib.pyplot as plt
import numpy as np
import scipy.signal as scp


def euler_step(A,B,C,dt,T):
    t = np.arange(0,T,dt)
    n = int (T/dt)

    x = np.zeros((A.shape[0],n+1))
    y = np.zeros((1,n))

    u=1

    for i in range (0,n):
        val = A @ x[: ,i]*dt + B * dt*u + x[: ,i]
        x[:, i+1] = val
        y[0, i] = C @ x[:, i]

    return t, y[0, :]


def euler_impulse(A,B,C,dt,T):
    t = np.arange(0,T,dt)
    n = int (T/dt)
    x = np.zeros((A.shape[0],n+1))
    y = np.zeros((1,n))
    u=1/dt

    for i in range(-1,n):
        x[: ,i+1] = x[:, i] + A @ x[:, i] * dt + B*dt*u
        y[0, i] = C @ x[:, i]
        u=0
    return t,y[0,: ]

if __name__ == '__main__':
    A = np.array([-3/5])
    B = np.array([1/5])
    C = np.array([1])
    #D = np.array([0])

    [tc,hc] = euler_step(A,B,C,0.1,10)
    plt.plot(tc,hc)
    plt.show()

    [tc1, hc1] = euler_impulse(A, B, C, 0.1, 10)
    plt.plot(tc1, hc1)
    plt.show()