import time

import numpy as np

from config.config import get_config
from .gx5_25 import GX5_25


def poll(n_reps, COM):
    euler = np.ndarray([3,1])     # [roll, pitch, yaw] in radians
    with GX5_25(COM) as ahrs:
        tic = time.time()             # used to measure the polling delay
        for n in range(n_reps):
            euler = ahrs.get_data()   # read the data
            print(np.rad2deg(euler))
        print("average dt = ", (time.time()-tic)/n_reps) # average polling delay

cfg = get_config(validate_present=['ahrs_com'])
if cfg:
    COM=cfg['ahrs_com']
else:
    COM = 'COM6'
            
poll(10, COM)