#LIBRERIE
from machine import Pin, ADC, I2C
from time import sleep_us, ticks_ms, ticks_diff
import ssd1306

#DEFINIZIONE DI FUNZIONI
def read(raw):
    
    x = sensor.read() * 3.3/4096
    raw = raw + [x]
    sleep_us(3900)
    #fc = 256.4 Hz

    return raw


def signal_init(L):

    raw = []

    while len(raw) < L:
       raw = read(raw)
    
    return raw


def fir_out(raw, response, L):

    output = 0
    sum_index = L - 1

    for i in range(L):
        for sum_index in range(L):
            output = output + response[i] * raw[sum_index]

    return output

#INIZIALIZAZIONE PIN E VARIABILI
sensor = ADC(32)

i2c = I2C(0)
display = ssd1306.SSD1306_I2C(128,64,i2c)
display.fill(0) 

L = 8
response = [0.227027158275565,	0.105622260802001,	-0.0157826366715633,	-0.137187534145127,	
-0.348432055749129,	-0.227027158275565,	-0.105622260802001,	0.0157826366715633]

j = 0

raw = signal_init(L)
filtered = [fir_out(raw, response, L)]

start_time = ticks_ms()

while True:

    raw = read(raw)
    filtered = filtered + [fir_out(raw, response, L)]

    hs_0 = int(raw[-2]*32/3.3)
    hs_1 = int(raw[-1]*32/3.3)

    hf_0 = int(filtered[-2]*16/3.3)
    hf_1 = int(filtered[-1]*18/3.3)

    display.line(j-1, 32 - hs_0, j, 32 - hs_1, 1)
    display.line(j-1, 48 - hf_0, j, 48 - hf_1, 1)

    if j<127:
        j = j + 1

    else:
        j = 0
        display.fill(0)
        
        #CONTROLLO SUL TEMPO
        end_time = ticks_ms()
        elapsed_time = ticks_diff(end_time, start_time)/1000
        print(elapsed_time)
        start_time = end_time
    
    if  (j != 0) and ((j % 8 == 0) or (j == 127)):
        display.show()
        
    raw.pop(0)
    filtered.pop(0)