from array import array
# create an incomming data array of 6 bytes
from machine import Pin, UART
import machine
import math
import time


I2C_SDA_PIN = 0
I2C_SCL_PIN = 1
i2c=machine.I2C(0,sda=machine.Pin(I2C_SDA_PIN), scl=machine.Pin(I2C_SCL_PIN), freq=400000)

# Compass I2C address (replace with your compass's address)
COMPASS_ADDRESS = 0x0D  

# Compass registers (replace with your compass's registers)
REG_OUT_X_MSB = 0x01
REG_OUT_Y_MSB = 0x03
REG_OUT_Z_MSB = 0x05

def from_signed_bytes(val, byteorder):
    result = int.from_bytes(val, byteorder)
    if result >= 2**(len(val)*8 - 1):
        result -= 2**(len(val)*8)
    return result


def read_compass():
    # Read X, Y, Z axis
    x = i2c.readfrom_mem(COMPASS_ADDRESS, REG_OUT_X_MSB, 2)
    y = i2c.readfrom_mem(COMPASS_ADDRESS, REG_OUT_Y_MSB, 2)
    z = i2c.readfrom_mem(COMPASS_ADDRESS, REG_OUT_Z_MSB, 2)
    print(x, y, z)
    # Convert the readings to integers
    x_val = from_signed_bytes(x, 'big')
    y_val = from_signed_bytes(y, 'big')
    z_val = from_signed_bytes(z, 'big')
    print(x_val, y_val, z_val)

    # Calculate heading
    heading = math.atan2(y_val, x_val)
    heading += math.pi

    # Convert to degrees
    heading_degrees = math.degrees(heading)
    return heading_degrees

class Compass():
    def __init__(self, i2c):
        self.i2c = i2c


# Example usage

while True:
    heading = read_compass()
    time.sleep(0.1)
    print("Heading: {:.2f} degrees".format(heading))