import cv2
import time
import board
import adafruit_bno055
import csv
import numpy as np
from math import atan2, pi, cos, sin, pi


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

points = []

def remap(value, maxInput, minInput, maxOutput, minOutput):
	value = maxInput if value > maxInput else value
	value = minInput if value < minInput else value
	inputSpan = maxInput - minInput
	outputSpan = maxOutput - minOutput
	scaledThrust = float(value - minInput) / float(inputSpan)
	return minOutput + (scaledThrust * outputSpan)

def draw_frame(heading, mag_data):
    mag_x, mag_y = mag_data
    global points
    frame = np.full(shape=[400, 400, 3], fill_value=255, dtype=np.uint8)
    length = 150
    P1 = (200, 200)
    P2_0 = int(P1[0] + length * cos(heading * pi / 180.0))
    P2_1 = int(P1[1] + length * sin(heading * pi / 180.0))
    P2 = (P2_0, P2_1)

    frame = cv2.line(frame,P1,P2,(255,0,0),2)
    frame = cv2.circle(frame, (mag_x, mag_y), radius=1, color=(0, 255, 0), thickness=3)

    for p in points:
        frame = cv2.circle(frame, (p[0], p[1]), radius=1, color=(0, 0, 255), thickness=3)
    points.append((mag_x, mag_y))
    frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE)

    font = cv2.FONT_HERSHEY_SIMPLEX
    org_n = (200, 30)
    org_s = (200, 370)
    org_w = (30, 200)
    org_e = (370, 200)
    fontScale = 1
    color = (255, 0, 0)
    thickness = 2
    frame = cv2.putText(frame, 'N', org_n, font, fontScale, color, thickness, cv2.LINE_AA)
    frame = cv2.putText(frame, 'S', org_s, font, fontScale, color, thickness, cv2.LINE_AA)
    frame = cv2.putText(frame, 'W', org_w, font, fontScale, color, thickness, cv2.LINE_AA)
    frame = cv2.putText(frame, 'E', org_e, font, fontScale, color, thickness, cv2.LINE_AA)

    cv2.imshow('direction', frame)

def get_heading():
    try:
        mag_data = sensor.magnetic
        for i in [0, 1]:
            if abs(mag_data[i]) > 20:
                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
        mag_x = int(remap(mag_data[0], 20, -20, 0, 400))
        mag_y = int(remap(mag_data[1], 20, -20, 0, 400))
        print(f'Heading: {heading}')
        return (heading+180, (mag_x, mag_y))
    except Exception as e:
        print(f"Encountered exception: {e}")
        return False

while True:
    heading = get_heading()
    if heading:
        draw_frame(heading[0], heading[1])
        cv2.waitKey(1)

    time.sleep(0.2)