# Project object: Acquire distance measurements and print the values to the console
#
# Hardware and connections used:
#   Ultrasonic Distance Sensor VCC to 3.3 V
#   Ultrasonic Distance Sensor to GND
#   Ultrasonic Distance Sensor TRIG Pin to GPIO Pin 15
#   Ultrasonic Distance Sensor ECHO Pin to GPIO Pin 14
#
# Programmer: Adrian Josele G. Quional

# modules
from picozero import DistanceSensor     # importing the DistanceSensor class to easily handle distance calculations
from time import sleep
import math

# creating a DistanceSensor object and specifying ECHO and TRIG pins
# any GPIO pin can be used for echo and trigger
ds = DistanceSensor(echo=14, trigger=15)

GRID_SIZE = 10
CELL_SIZE_CM = 10

def create_empty_grid():
    return [[0 for _ in range(GRID_SIZE)] for _ in range(GRID_SIZE)]

def update_map(distance_cm, angle_deg=0):
    grid = create_empty_grid()

    # ตำแหน่งหุ่นยนต์ตรงกลาง
    robot_x = GRID_SIZE // 2
    robot_y = GRID_SIZE // 2
    grid[robot_y][robot_x] = 9

    # คำนวณตำแหน่งสิ่งกีดขวาง
    dx = math.cos(math.radians(angle_deg)) * distance_cm
    dy = math.sin(math.radians(angle_deg)) * distance_cm
    target_x = int(robot_x + dx / CELL_SIZE_CM)
    target_y = int(robot_y - dy / CELL_SIZE_CM)

    # ถ้าตำแหน่งยังอยู่ในแผนที่ ให้กำหนดเป็น 1
    if 0 <= target_x < GRID_SIZE and 0 <= target_y < GRID_SIZE:
        grid[target_y][target_x] = 1

    return grid

def print_grid(grid):
    print("Map (0=ว่าง,1=สิ่งกีดขวาง,9=หุ่นยนต์):")
    for row in grid:
        print(" ".join(str(cell) for cell in row))
    print("\n")

# ตัวอย่างใช้งานกับค่าระยะที่เปลี่ยนไป
angle = 90  # ทิศทาง sensor มองไปทางขวา

# continuously acquire distance measurements while the board has power
while True:
    # printing the values to the console
    # values are converted to cm because distance method returns values in m
    # conversion factor used is 100 cm = 1 m
    distance_cm = ds.distance * 100 
    map_grid = update_map(distance_cm, angle)
    print_grid(map_grid)

    # print(f"Distance: {distance_cm} cm")
    sleep(5)