import sys, getopt


sys.path.append('.')
import RTIMU
import os.path
import time
import math
# from .math3d import *
import euclid3



SETTINGS_FILE = "RTIMULib"

print("Using settings file " + SETTINGS_FILE + ".ini")
if not os.path.exists(SETTINGS_FILE + ".ini"):
  print("Settings file does not exist, will be created")

s = RTIMU.Settings(SETTINGS_FILE)
imu = RTIMU.RTIMU(s)

print("IMU Name: " + imu.IMUName())

if (not imu.IMUInit()):
    print("IMU Init Failed")
    sys.exit(1)
else:
    print("IMU Init Succeeded")

# this is a good time to set any fusion parameters

imu.setSlerpPower(0.02)
imu.setGyroEnable(True)
imu.setAccelEnable(True)
imu.setCompassEnable(True)

poll_interval = imu.IMUGetPollInterval()
print("Recommended Poll Interval: %dmS\n" % poll_interval)

q = euclid3.Quaternion()

while True:
    if imu.IMURead():
        # x, y, z = imu.getFusionData()
        # print("%f %f %f" % (x,y,z))
        data = imu.getIMUData()
        fusionPose = data["fusionQPose"]
        q.w = fusionPose[0]
        q.x = fusionPose[1]
        q.y = fusionPose[2]
        q.z = fusionPose[3]
        print(math.atan2(2*q.y*q.w-2*q.x*q.z,1-2*q.y*q.y -2*q.z*q.z))
        # print(math.asin(2*(q.x * q.y + q.w * q.z)))
        # print(q.w,q.x,q.y,q.z)
        # print(q.get_euler())
        # print("r: %f p: %f y: %f" % (math.degrees(fusionPose[0]),
        #     math.degrees(fusionPose[1]), math.degrees(fusionPose[2])))
        time.sleep(poll_interval*1.0/1000.0)




