#!/usr/bin/env python

from icm20948 import ICM20948
import time
import math

print("""magnetometer.py - Convert raw values to heading
Rotate the sensor (X-axis upwards) through 360 degrees to calibrate.
Press Ctrl+C to exit!
""")

X = 0
Y = 1
Z = 2

AXES = Y, X

imu = ICM20948()

amin = list(imu.read_magnetometer_data())
amax = list(imu.read_magnetometer_data())

tolerance = 5     # How many degrees from exact to match
isNorth = False
isSouth = False

while True:
    mag = list(imu.read_magnetometer_data())
    for i in range(3):
        v = mag[i]
        if v < amin[i]:
            amin[i] = v
        if v > amax[i]:
            amax[i] = v
        mag[i] -= amin[i]
        try:
            mag[i] /= amax[i] - amin[i]
        except ZeroDivisionError:
            pass
        mag[i] -= 0.5

    heading = math.atan2(
            mag[AXES[0]],
            mag[AXES[1]])

    if heading < 0:
        heading += 2 * math.pi
    heading = math.degrees(heading)
    heading = round(heading)
    
if (heading >= 360 - 5) or (heading <= 0 + 5):
    # 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 - 5) and (heading <= 180 + 5):
    # 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
    isSouth = False

    print("Heading: {}".format(heading))

    time.sleep(0.5)