#!/usr/bin/python


import time
import math
from icm20948 import ICM20948
import datetime
import os
import sys


RAD_TO_DEG = 57.29578
M_PI = 3.14159265358979323846
G_GAIN = 0.070  # [deg/s/LSB]  If you change the dps for gyro, you need to update this value accordingly
AA =  0.40      # Complementary filter constant

################# Compass Calibration values ############
# Use calibrateBerryIMU.py to get calibration values
# Calibrating the compass isnt mandatory, however a calibrated
# compass will result in a more accurate heading values.

magXmin =  0
magYmin =  0
magZmin =  0
magXmax =  0
magYmax =  0
magZmax =  0



'''
Here is an example:
magXmin =  -1748
magYmin =  -1025
magZmin =  -1876
magXmax =  959
magYmax =  1651
magZmax =  708
Dont use the above values, these are just an example.
'''
############### END Calibration offsets #################



gyroXangle = 0.0
gyroYangle = 0.0
gyroZangle = 0.0
CFangleX = 0.0
CFangleY = 0.0


IMU = ICM20948()


a = datetime.datetime.now()



while True:
    #Read the accelerometer,gyroscope and magnetometer values
        x, y, z = IMU.read_magnetometer_data()
        ax, ay, az, gx, gy, gz = IMU.read_accelerometer_gyro_data()

    #Apply compass calibration
        x -= (magXmin + magXmax) /2
        y -= (magYmin + magYmax) /2
        z -= (magZmin + magZmax) /2

    ##Calculate loop Period(LP). How long between Gyro Reads
        b = datetime.datetime.now() - a
        a = datetime.datetime.now()
        LP = b.microseconds/(1000000*1.0)
        outputString = "Loop Time %5.2f " % ( LP )


    #Convert Gyro raw to degrees per second
        gx =  gx * G_GAIN
        gy =  gy * G_GAIN
        gz =  gz * G_GAIN


    #Calculate the angles from the gyro.
        gx+=gx*LP
        gy+=gy*LP
        gz+=gz*LP


    #Convert Accelerometer values to degrees
        ax =  (math.atan2(ay,az)*RAD_TO_DEG)
        ay=  (math.atan2(az,ax)+M_PI)*RAD_TO_DEG

    #convert the values to -180 and +180
        if ay > 90:
            ay -= 270.0
        else:
            ay += 90.0



    #Complementary filter used to combine the accelerometer and gyro values.
        CFangleX=AA*(CFangleX+gx*LP) +(1 - AA) * ax
        CFangleY=AA*(CFangleY+gy*LP) +(1 - AA) * ax



    #Calculate heading
        heading = 180 * math.atan2(y,x)/M_PI

    #Only have our heading between 0 and 360
        if heading < 0:
            heading += 360

    ####################################################################
    ###################Tilt compensated heading#########################
    ####################################################################
    #Normalize accelerometer raw values.
        axnorm = ax/math.sqrt(ax * ax + ay * ay + az * az)
        aynorm = ay/math.sqrt(ax * ax + ay * ay + az * az)


    #Calculate pitch and roll
        pitch = math.asin(axnorm)
        roll = -math.asin(aynorm/math.cos(pitch))


    #Calculate the new tilt compensated values
    #The compass and accelerometer are orientated differently on the the BerryIMUv1, v2 and v3.
    #This needs to be taken into consideration when performing the calculations

    #X compensation
    #if(IMU.BerryIMUversion == 1 or IMU.BerryIMUversion == 3):            #LSM9DS0 and (LSM6DSL & LIS2MDL)
        #magXcomp = MAGx*math.cos(pitch)+MAGz*math.sin(pitch)
        #else:                                                                #LSM9DS1
            #magXcomp = MAGx*math.cos(pitch)-MAGz*math.sin(pitch)

    #Y compensation
        #if(IMU.BerryIMUversion == 1 or IMU.BerryIMUversion == 3):            #LSM9DS0 and (LSM6DSL & LIS2MDL)
            #magYcomp = MAGx*math.sin(roll)*math.sin(pitch)+MAGy*math.cos(roll)-MAGz*math.sin(roll)*math.cos(pitch)
        #else:                                                                #LSM9DS1
            #magYcomp = MAGx*math.sin(roll)*math.sin(pitch)+MAGy*math.cos(roll)+MAGz*math.sin(roll)*math.cos(pitch)




    #Calculate tilt compensated heading
        tiltCompensatedHeading = 180 * math.atan2(y,x)/M_PI

        if tiltCompensatedHeading < 0:
            tiltCompensatedHeading += 360


    ##################### END Tilt Compensation ########################


        if 1:                       #Change to '0' to stop showing the angles from the accelerometer
            outputString += "#  ax Angle %5.2f ay Angle %5.2f  #  " % (ax, ay)

        if 0:                       #Change to '0' to stop  showing the angles from the gyro
            outputString +="\t# gx Angle %5.2f  gy Angle %5.2f  GYRZ Angle %5.2f # " % (gx,gy,gz)

        if 0:                       #Change to '0' to stop  showing the angles from the complementary filter
            outputString +="\t#  CFangleX Angle %5.2f   CFangleY Angle %5.2f  #" % (CFangleX,CFangleY)

        if 1:                       #Change to '0' to stop  showing the heading
            outputString +="\t# HEADING %5.2f  tiltCompensatedHeading %5.2f #" % (heading,tiltCompensatedHeading)


        print(outputString)



    #slow program down a bit, makes the output more readable
        time.sleep(1)
        

        tolerance = 10     # How many degrees from exact to match
        isNorth = False
        isSouth = False
        
        
        if (heading >= 360 - 10) or (heading <= 0 + 10):
        # Pointing north
            if not isNorth:
            # This runs once each time you start facing north
             print("North")
        # This runs each loop while pointing north
             isNorth = True
             isSouth = False
        elif (heading >= 180 - 10) and (heading <= 180 + 10):
        # Pointing south
            if not isSouth:
        # This runs once each time you start facing south
             print("South")
        # This runs each loop while pointing south
             isNorth = False
             isSouth = True
         
        else:
        # Not pointing at any of the above
             isNorth = False