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

from packet_parser_helpers import *
                
def update_with_data(ser, types):
    
    ser.timeout = 3
    ser.reset_input_buffer()
    start = time.time()
    while(True):
        ser.write(b'r')
        size = int.from_bytes(ser.read(4), byteorder = 'little', signed = False)
        cons = ser.read(size)
        read_and_process(types, cons, size)
            # if t['type_name'] == b'packet_ekg' and e['name'] == b'readings_cnts[50]':
            #     chunked = [(2.4 / (1<<24)) * int.from_bytes(block[i:i + element_size], byteorder='little', signed = True) for i in range(0, len(block), element_size)]
            #     adcs += chunked
            #         if len(adcs) > 500:
            #             dat = np.array(adcs[-2048:])
            #             if len(adcs) > 2048:
            #                 adcs = adcs[-2048:]
            #             #filtered_dat = my_filter(dat)
            #             filtered_dat = dat
            #             filtered_dat = signal.filtfilt(b, a, filtered_dat)
            #             dd = np.sort(dat)
            #             center = 0.5 * (dd[-100] + dd[100])
            #             range_ = dd[-100] - dd[100]
            #             if len(dat) == 2048:
            #                 print("Freq:", 2048 / (time.time() - start))
            #                 axs[0].cla()
            #                 axs[1].cla()
            #                 axs[0].set_title("ADC {:3.3f}mV".format(1000 * (range_)))
            
            #                 #axs[0].plot(dat, 'r.', linestyle = '--')
            #                 axs[0].plot(filtered_dat, 'k.', linestyle = '--')
            #                 #axs[0].set_ylim(center - range_, center + range_)
            #                 _fft = np.log(np.abs(np.fft.fft(filtered_dat)[1:]))
            #                 axs[1].plot(250 * np.fft.fftfreq(dat.shape[-1])[1:], _fft, 'k.')
            #                 axs[1].axvline(x = 60)
            #                 plt.savefig("image.png")
            
            # if t['type_name'] == b'packet_imu' and e['name'] == b'readings_cnts[4]':
            #     imu_reading_type = block[0] >> 3
            #     imu_reading_tag_cnt = (block[0] >> 1) & 3
            #     reading_xyz = [int.from_bytes(block[2:4], byteorder = 'big', signed = True),
            #                    int.from_bytes(block[4:6], byteorder = 'big', signed = True),
            #                    int.from_bytes(block[6:8], byteorder = 'big', signed = True)]
            #     if imu_reading_type == 1:
            #         #print("{:02x} {:d}".format(imu_reading_type, imu_reading_tag_cnt))
            #         gyros.append([250 * e / (1<<16) for e in reading_xyz])
            #     elif imu_reading_type == 2:
            #         accs.append([4 * e / (1<<16) for e in reading_xyz])
            #     else:
            #         print("ERR")


def main():

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

    ser.write(b'?')
    lines = []
    for i in range(13):
        lines.append(ser.readline().strip(b'\n').strip(b'\r'))
    
    types = get_type_list(lines)

    # ser.write(b'1')
    # ser.write(b'3')
    # ser.write(b'5')
    ser.flush()
    
    update_with_data(ser, types)

main()