import time
import board
import adafruit_bno055
import csv
from math import atan2, pi


i2c = board.I2C()
sensor = adafruit_bno055.BNO055_I2C(i2c)


def start_recording():
    with open('results.csv', 'w', newline='') as csvfile:
            csvwriter = csv.writer(csvfile, delimiter=',', quotechar='|', quoting=csv.QUOTE_MINIMAL)
            csvwriter.writerow(['time', 'x', 'y', 'z'])
    print("Starting recording for 60s.")
    end_time = time.time() + 60
    while time.time() < end_time:
        taking_measurement_end = time.time() + 0.05
        try:
            mag_data = sensor.magnetic
            do_write = True
            for i in [0, 1]:
                if abs(mag_data[i]) > 15:
                    print(f"Got {mag_data[i]} on {'X' if i == 0 else 'Y' if i==1 else 'Z'}, skipping")
                    do_write = False
            if do_write:
                with open('results.csv', 'a', newline='') as csvfile:
                    csvwriter = csv.writer(csvfile, delimiter=',', quotechar='|', quoting=csv.QUOTE_MINIMAL)
                    csvwriter.writerow([time.time(), mag_data[0], mag_data[1], mag_data[2]])
            while time.time() < taking_measurement_end:
                pass
        except Exception as e:
            print(f"Encountered exception: {e}")
            continue

    print("Done")

def get_heading():
    try:
        mag_data = sensor.magnetic
        for i in [0, 1]:
            if abs(mag_data[i]) > 15:
                print(f"Got {mag_data[i]} on {'X' if i == 0 else 'Y' if i==1 else 'Z'}, skipping")
                return False
        heading = 180 * atan2(mag_data[1],mag_data[0])/pi
        if heading < 0:
            heading += 360
        print(f"Heading: {heading}")
    except Exception as e:
        print(f"Encountered exception: {e}")
        return False


start_recording()

#while True:
#    get_heading()
#    time.sleep(0.05)