import sys
from nbus_slave import *
from nbus_system import *
import matplotlib.pyplot as plt
from imu_slave import *
import numpy as np

def show_data(data):
    fig, ax = plt.subplots()
    for d in data:
        ax.plot(data[d])
    ax.grid()
    plt.show()


if __name__ == "__main__":

    nbus = NbusSystem(False)
    s1_adr = 5
    s2_adr = 6
    adrs = [s1_adr, s2_adr]

    foundSlaves = {}
    for adr in adrs:
        slave1 = nbus.create_slave(ImuSlave, adr)
        foundSlaves[adr] = True

    startCheck = True
    for adr in adrs:
        if nbus.get_slave(adr).cmd_echo([97, 98, 99, 100]) == 0:
            foundSlaves[adr] = False
    for s in foundSlaves:
        startCheck = startCheck and foundSlaves[s]

    print("Module presence:", foundSlaves)

    if not startCheck:
        sys.exit()

    # app.cmd_reset()
    store_param = False
    if store_param:
        nbus.get_slave(s1_adr).cmd_set_param(1, PARAM_SAMPLERATE, 1)
        nbus.get_slave(s1_adr).cmd_set_param(1, PARAM_RANGE, 0)
        nbus.get_slave(s1_adr).cmd_set_param(1, PARAM_FILTER, 2)

        nbus.get_slave(s1_adr).cmd_store(1)

        sys.exit()

    # !!!! slave1.cmd_set_param(1, PARAM_SAMPLERATE, 10)
    # !!!! slave1.cmd_set_param(2, PARAM_SAMPLERATE, 10)
    for adr in adrs:
        for s in range(1):
            sr = nbus.get_slave(adr).cmd_get_param(s + 1, PARAM_SAMPLERATE)
            r = nbus.get_slave(adr).cmd_get_param(s + 1, PARAM_RANGE)
            lpf = nbus.get_slave(adr).cmd_get_param(s + 1, PARAM_FILTER)
            ee = nbus.get_slave(adr).cmd_get_param(s + 1, PARAM_GAIN)
            print(sr, r, lpf, ee)

        # sr = nbus.get_slave(adr).cmd_get_param(1, PARAM_SAMPLERATE)

        # nbus.get_slave(adr).cmd_set_param(1, PARAM_RANGE, 1)
        # print(nbus.get_slave(adr).slave_params)

    pocet = {}
    for adr in adrs:
        pocet[adr] = nbus.get_slave(adr).cmd_sensor_cnt()
        # sys.exit()
        print("pocet senzorov=", pocet)

    nbus.cmd_start()
    time.sleep(0.1)
    #nbus.cmd_stop()
    #sys.exit()

    data_acc = {}
    for adr in adrs:
        data_acc[adr] = []
    print(data_acc)
    for i in range(64):
        for adr in adrs:
            acc = nbus.get_slave(adr).cmd_sensor_get_data_IMU(1)
            # gyr = nbus.get_slave(adr).cmd_sensor_get_data_IMU(2)
            data_acc[adr].append(acc)
            # print(acc)
        print(".", end=" ")
    nbus.cmd_stop()
    show_data(data_acc)

    # sys.exit()

    for adr in adrs:
        for i in range(pocet[adr]):
            d = nbus.get_slave(adr).cmd_sensor_get_data(i + 1)
            print("Data", i + 1, ":\t", d)

        print("Sensor type")
        for i in range(pocet[adr]):
            print(nbus.get_slave(adr).cmd_sensor_type(i + 1))
        print("Sensors type")
        print(nbus.get_slave(adr).cmd_sensors_type(pocet[adr]))

        par = nbus.get_slave(adr).cmd_get_params(1)
        print(par)

        r = nbus.get_slave(adr).cmd_module_info(0xE1)
        print('NAME:', r)
        r = nbus.get_slave(adr).cmd_module_info(0xE2)
        print('TYPE', r)

        r = nbus.get_slave(adr).cmd_module_info(0xE4)
        print('FW', r)

        r = nbus.get_slave(adr).cmd_module_info(0xE5)
        print('HW', r)

        r = nbus.get_slave(adr).cmd_module_info(0xE3)
        print("UUID", r)

        r = nbus.get_slave(adr).cmd_module_info(0xE6)
        print("MEM ID", r)

    nbus.finish()