import time
import RPi.GPIO as GPIO
import sys
sys.path.append('.')
import RTIMU

class Compass:

    def __init__(self):

        self.SETTINGS_FILE = "RTIMULib"
        s = RTIMU.Settings(self.SETTINGS_FILE)
        self.imu = RTIMU.RTIMU(s)


        if (not self.imu.IMUInit()):
            print('IMU init failed.')
            sys.exit(1)

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

        print('IMU init successful.')
        self.poll_interval = self.imu.IMUGetPollInterval()
        print('poll interval: ', self.poll_interval)


    def getReading(self):

        try:
            while True:

                if self.imu.IMURead():
                    data = self.imu.getIMUData()
                    fusion_pose = data['fusionPose']

                    time.sleep(self.poll_interval*1.0/1000.0)
                    return(fusion_pose)

        except:
            print('error in getting IMU reading')