from machine import Pin, SPI
from ili9341 import Display
from machine import UART
import struct, time

"""
SCHEMAT PODŁĄCZENIA:
    EKRAN:
        VCC   -> 3.3V
        GND   -> GND
        CD    -> D5
        RESET -> D4
        DS    -> D2
        SDI   -> D23
        SCK   -> D18
        LED   -> 3.3V
        SDO   -> D19
    CZUJNIK:
        VCC -> VIN
        GND -> GND
        RX  -> TX2
        TX  -> RX2
"""


#================== LED SCREEN SETUP ==================
spi = SPI(2, baudrate=40000000, polarity=0, phase=0,
          sck=Pin(18), mosi=Pin(23), miso=Pin(19))
cs = Pin(5, Pin.OUT)
dc = Pin(2, Pin.OUT)
rst = Pin(4, Pin.OUT)

display = Display(spi, cs=cs, dc=dc, rst=rst, width=240, height=320, rotation=0)
display.display_on()
display.clear()

#================== UART / LD2450 SETUP ==================
stuart = UART(2, baudrate=256000, tx=17, rx=16)
time.sleep(0.5)

enable_output = bytes([0xFD,0xFC,0xFB,0xFA,0x04,0x00,0x01,0x00,0x00,0x00,0x01,0x00,0x00,0x00,0x04,0x00,0xFA,0xFB,0xFC,0xFD])
always_on     = bytes([0xFD,0xFC,0xFB,0xFA,0x04,0x00,0x01,0x00,0x00,0x00,0x03,0x00,0x00,0x00,0x06,0x00,0xFA,0xFB,0xFC,0xFD])
single_mode   = bytes([0xFD,0xFC,0xFB,0xFA,0x04,0x00,0x01,0x00,0x00,0x00,0x02,0x00,0x00,0x00,0x05,0x00,0xFA,0xFB,0xFC,0xFD])
query_once    = bytes([0xFD,0xFC,0xFB,0xFA,0x01,0x00,0x10,0x00,0x11,0x00,0xFA,0xFB,0xFC,0xFD])

stuart.write(enable_output)
stuart.write(always_on)
stuart.write(single_mode)

HEADER = b'\xAA\xFF\x03\x00'
FOOTER = b'\x55\xCC'
TARGET_SIZE = 8
MAX_TARGETS = 3
NUM_READINGS = 10

#================== FUNCTIONS ==================
def read_packet(uart):
    while True:
        b = uart.read(1)
        if not b:
            continue
        if b == HEADER[:1]:
            rest = uart.read(3)
            if rest == HEADER[1:]:
                break
    packet = bytearray()
    while True:
        b = uart.read(1)
        if not b:
            continue
        packet += b
        if packet.endswith(FOOTER):
            return packet[:-2]

def parse_ld2450(packet):
    targets = []
    offset = 0
    for _ in range(MAX_TARGETS):
        if offset + TARGET_SIZE > len(packet):
            break
        raw_x = struct.unpack_from("<H", packet, offset)[0]
        raw_y = struct.unpack_from("<H", packet, offset + 2)[0]
        raw_speed = struct.unpack_from("<H", packet, offset + 4)[0]
        distance = struct.unpack_from("<H", packet, offset + 6)[0]

        x = (raw_x & 0x7FFF) if (raw_x & 0x8000) else -(raw_x & 0x7FFF)
        y = (raw_y & 0x7FFF) if (raw_y & 0x8000) else -(raw_y & 0x7FFF)
        magnitude = raw_speed & 0x7FFF
        speed = magnitude if (raw_speed & 0x8000) else -magnitude

        offset += TARGET_SIZE
        if x == 0 and y == 0 and speed == 0 and distance == 0:
            continue
        targets.append((x, y, speed, distance))
    return targets

def get_reading(uart):
    uart.write(query_once)
    return read_packet(uart)

def color(r, g, b):
    return (r & 0xf8) << 8 | (g & 0xfc) << 3 | b >> 3

def draw_radar_field(display, coords):
    # ----- FIELD SETTINGS -----
    FIELD_X = 15
    FIELD_Y = 10
    FIELD_W = 220   # smaller width to avoid overlapping text
    FIELD_H = 235   # smaller height
    CENTER_X = FIELD_W // 2
    BOTTOM_Y = FIELD_H
    SENSOR_X_MAX = 2000
    SENSOR_Y_MAX = 2900

    COLOR_BG = 0x0000      # black
    COLOR_BORDER = 0xFFFF  # white
    COLOR_MARK = 0xF800    # red
    COLOR_REF = 0x07E0     # green for reference

    # ----- CLEAR FIELD -----
    display.clear()

    # ----- DRAW FIELD BORDER -----
    display.draw_rectangle(FIELD_X, FIELD_Y, FIELD_W, FIELD_H, COLOR_BORDER)

    # ----- DRAW REFERENCE POINT (sensor) -----
    origin_x = FIELD_X + CENTER_X
    origin_y = FIELD_Y + BOTTOM_Y - 1
    display.draw_line(origin_x - 3, origin_y, origin_x + 3, origin_y, COLOR_REF)
    display.draw_line(origin_x, origin_y - 3, origin_x, origin_y + 3, COLOR_REF)
    display.draw_text8x8(origin_x + 5, origin_y - 5, "S", COLOR_REF)

    # ----- DRAW TARGETS -----
    for idx, (x_mm, y_mm, _, _) in enumerate(coords):
        # convert mm → field pixels
        x_px = int(CENTER_X + (x_mm / SENSOR_X_MAX) * CENTER_X)
        y_px = int(BOTTOM_Y - (y_mm / SENSOR_Y_MAX) * FIELD_H)

        # clip to field
        if not (0 <= x_px < FIELD_W and 0 <= y_px < FIELD_H):
            continue

        sx = FIELD_X + x_px
        sy = FIELD_Y + y_px

        # draw smaller rectangle for target
        display.draw_rectangle(sx-2, sy-2, 4, 4, COLOR_MARK)

        # draw target number above the point
        display.draw_text8x8(sx-2, sy-10, f"{idx+1}", COLOR_BORDER)

    # ----- DRAW TARGET INFO BELOW FIELD -----
    y_text = FIELD_Y + FIELD_H + 5
    for i, (x_mm, y_mm, speed, distance) in enumerate(coords):
        txt = f"T{i+1}: x={x_mm}, y={y_mm}, sp={speed}"
        display.draw_text8x8(FIELD_X+5, y_text + 15+i*15, txt, COLOR_BORDER)


# Track which targets were previously visible
prev_visible = [False] * MAX_TARGETS

#================== MAIN LOOP ==================
while True:
    sum_targets = [[0, 0, 0, 0] for _ in range(MAX_TARGETS)]
    count_targets = [0] * MAX_TARGETS

    # Take multiple readings for averaging
    for _ in range(NUM_READINGS):
        packet = get_reading(stuart)
        targets = parse_ld2450(packet)
        for i, target in enumerate(targets):
            if i >= MAX_TARGETS:
                break
            x, y, speed, distance = target
            sum_targets[i][0] += x
            sum_targets[i][1] += y
            sum_targets[i][2] += speed
            sum_targets[i][3] += distance
            count_targets[i] += 1
        time.sleep(0.03)

    avg_targets = []
    print("=== Targets ===")

    # Clear missing targets on LED
    for i in range(MAX_TARGETS):
        if count_targets[i] == 0 and prev_visible[i]:
            # Overwrite previous text with background
            y_text = 10 + 220 + 5 + 15 + i*15  # same position as in draw_radar_field
            display.draw_rectangle(10, y_text, 200, 12, 0x0000)  # black rectangle to erase
            prev_visible[i] = False
            print(f"Target {i+1} disappeared")

    # Draw/print current targets
    for i in range(MAX_TARGETS):
        if count_targets[i] == 0:
            if not prev_visible[i]:
                print(f"Target {i+1}: No detection")
            continue

        avg_x = int(sum_targets[i][0] / count_targets[i])
        avg_y = int(sum_targets[i][1] / count_targets[i])
        avg_speed = int(sum_targets[i][2] / count_targets[i])
        avg_distance = int(sum_targets[i][3] / count_targets[i])
        avg_targets.append((avg_x, avg_y, avg_speed, avg_distance))

        # Print to console
        print(f"Target {i+1}: x={avg_x}, y={avg_y}, speed={avg_speed}, distance={avg_distance}")
        prev_visible[i] = True

    print("------------------")

    # Draw on LED screen
    draw_radar_field(display, avg_targets)
    time.sleep(0.5)