import serial
import matplotlib.pyplot as plt
import numpy as np
from scipy import signal

#should be running at 244Hz

fs = 244
b, a = signal.butter(
    N = 2,
    Wn = [30],
    btype = 'lowpass',
    fs = fs)

ser = serial.Serial(port='/dev/ttyACM1', baudrate=115200, timeout = 0.5)
ser.flush()

data = bytes()

ready = False

ser.write(b'?')

types = []

def arr_sizes(s):
    if s[-1:] != b']' or b'[' not in s:
        return 1
    v = int(s[s.rindex(b'[') + 1:-1])
    return v

for i in range(13):
    line = ser.readline().strip(b'\n').strip(b'\r')
    if line != b'':
        L = line.split(b" ")
        types.append({'type_name' : L[0],
                      'type_code' : int(L[1]),
                      'size' : int(L[2]),
                      'elements' : []
                      })
        i = 3
        while i < len(L):
            types[-1]['elements'].append({'type_name' : L[i], 'name' : L[i + 1], 'offset' : int(L[i + 2]), 'n_elements' : arr_sizes(L[i + 1]), 'size' : int(L[i + 3]) * arr_sizes(L[i + 1]), 'readings' : []})
            i += 4
            
ser.flush()
ser.timeout = 5

ys = []

for i in range(1000):
    ser.write(b'R')
    data = ser.read(1024)
    index = 0
    while (index < 1024):
        packet_type = (data[index + 1]<<8) + data[index]
        ind = [i for i,t in enumerate(types) if t['type_code'] == packet_type]
        if len(ind) != 1:
            break
        t = types[ind[0]]
        d = data[index + 2 : index + 2 + t['size']]
        for e in t['elements']:
            block = d[e['offset']:e['offset'] + e['size']]
            element_size = int(len(block) / e['n_elements'])
            if t['type_name'] == b'packet_ekg' and e['name'] == b'readings_cnts[50]':
                chunked = [int.from_bytes(block[i:i + element_size - 1], byteorder='little', signed = True) for i in range(0, len(block), element_size)]
                #BB = [block[i:i + element_size] for i in range(0, len(block), element_size)]
                #if t['type_name'] == b'packet_ekg' and e['name'] == b'readings_cnts[50]':
                ys += chunked
                plt.clf()
                dat = np.array(ys[-1000:])
                plt.plot(dat, 'k.', linestyle = '--')#signal.filtfilt(b, a, dat))
                plt.pause(0.01)
                ##print(BB)
                e['readings'].append(chunked)
        index += 2 + t['size']

#for t in types:
#    if t['type_name'] == b'packet_ekg':
#        print(t)