import serial
import cv2
import numpy as np
import time

# Attempt to open the serial port
try:
    arduinoSerialData = serial.Serial('COM3', 19200)
except serial.SerialException as e:
    print(f"Failed to open serial port: {e}")
    exit()

# Constants
width, height = 700, 600
center = (width // 2, height // 2)
max_distance = 220  # Max distance in cm
angle_increment = 120  # Angle between lines in degrees q

# Create a blank image
image = np.zeros((height, width, 3), dtype=np.uint8)

# List to store the dots
dots = []

def draw_radar(image, center, distances, angle):
    # Clear the image
    image[:] = (0, 0, 0)
    
    # Draw the radar circles
    cv2.circle(image, center, 300, (0, 255, 0), 1)
    cv2.circle(image, center, 200, (0, 255, 0), 1)
    cv2.circle(image, center, 100, (0, 255, 0), 1)

    # Draw the radar lines and store dots
    for i in range(3):
        real_angle = angle + i * angle_increment
        angle_rad = np.deg2rad(real_angle)
        distance = distances[i]
        
        # Calculate end point of radar line
        end_point = (
            int(center[0] + max_distance * np.cos(angle_rad)),
            int(center[1] - max_distance * np.sin(angle_rad))
        )
        
        # Draw radar line
        cv2.line(image, center, end_point, (0, 255, 0), 1)
        
        # Calculate the position of the distance dot
        dot_point = (
            int(center[0] + distance * np.cos(angle_rad)),
            int(center[1] - distance * np.sin(angle_rad))
        )
        
        # Store the dot position and the real angle
        dots.append((dot_point, real_angle))

    # Draw the stored dots
    for dot, angle in dots:
        cv2.circle(image, dot, 3, (0, 0, 255), -1)


while True:
    if arduinoSerialData.in_waiting > 0:
        try:
            # Read the incoming data
            rawData = arduinoSerialData.readline()
            
            # Filter out invalid bytes
            myData = rawData.decode('utf-8', errors='ignore').strip()
            
            # Split the string into individual components
            data_parts = myData.split(',')
            
            # Convert the string components to integers or floats as needed
            try:
                distance1 = int(data_parts[0])
                distance2 = int(data_parts[1])
                distance3 = int(data_parts[2])
                angle = int(data_parts[3])
            except (ValueError, IndexError) as e:
                print(f"Data conversion error: {e}")
                continue
            
            # Reset dots at the beginning of a new sweep
            if angle == 0 or angle == 14:
                dots.clear()
                
            # Convert angle to range 0-120 degrees
            angle_converted = angle * 120 / 14
            
            # Draw the radar
            draw_radar(image, center, [distance1, distance2, distance3], angle_converted)
            
            # Show the image
            cv2.imshow('Radar', image)
            
            # Wait for a short period to simulate real-time data updates
            if cv2.waitKey(100) & 0xFF == ord('q'):
                break
            time.sleep(0.1)  # Delay for 100ms to simulate real-time data updates
            
        except Exception as e:
            print(f"Error: {e}")
            continue

cv2.destroyAllWindows()