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()
    ax.plot(data_acc)
    ax.grid()
    plt.show()


if __name__ == "__main__":

    nbus = NbusSystem(False)
    s1_adr = 5
    slave1 = nbus.create_slave(ImuSlave, s1_adr)

    if nbus.get_slave(s1_adr).cmd_echo([97, 98, 99, 100]) == 0:
        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 s in range(1):
        sr = nbus.get_slave(s1_adr).cmd_get_param(s + 1, PARAM_SAMPLERATE)
        r = nbus.get_slave(s1_adr).cmd_get_param(s + 1, PARAM_RANGE)
        lpf = nbus.get_slave(s1_adr).cmd_get_param(s + 1, PARAM_FILTER)
        ee = nbus.get_slave(s1_adr).cmd_get_param(s + 1, PARAM_GAIN)
        print(sr, r, lpf, ee)

    sr = nbus.get_slave(s1_adr).cmd_get_param(1, PARAM_SAMPLERATE)

    nbus.get_slave(s1_adr).cmd_set_param(1, PARAM_RANGE, 1)
    print(nbus.get_slave(s1_adr).slave_params)

    nbus.cmd_start()
    time.sleep(0.1)

    # nbus.cmd_stop()
    # sys.exit()
    pocet = nbus.get_slave(s1_adr).cmd_sensor_cnt()
    # sys.exit()
    print("pocet senzorov=", pocet)

    data_acc = []
    for i in range(164):
        acc = nbus.get_slave(s1_adr).cmd_sensor_get_data_IMU(1)
        # gyr = nbus.get_slave(s1_adr).cmd_sensor_get_data_IMU(2)
        data_acc.append(acc)
        # print(gyr)
        print(".", end=" ")
    nbus.cmd_stop()
    show_data(data_acc)

    # sys.exit()

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

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

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

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

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

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

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

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

    nbus.finish()