import time

# Solar module for simulation of world
import solar # Modeling of the world

from motor import Motor # Small helper functions and constants
from motor import Board

STEP = 10
LOOP_DELAY = 0.01 # In seconds

# Testing embedding the mirrors in the world
board = Board()
world = solar.World(board, tilt_deg=15)  # The world is tilted 15 degrees around y-axis

HEIGHT = 30

source = solar.Source(world, pos=(0, 0, 30))
target = solar.Target(world, pos=(20, 0, 30))

# Create mirrors in a 9x9 grid
for x in range(3):
    for y in range(3):
        mirror = solar.Mirror(world, cluster_x=x, cluster_y=y)
        world.add_mirror(mirror)

world.update_mirrors_from_source_target(source, target)

for i, mirror in enumerate(world.mirrors):
    pitch, yaw = mirror.get_angles()
    print(f"Mirror {i} ({mirror.cluster_x}, {mirror.cluster_y}) angles -> pitch: {pitch:.2f}°, yaw: {yaw:.2f}°")

# Main
try:
    while True:
        # Shutdown
        if GPIO.input(SHUTDOWN_BTN) == GPIO.HIGH:
            os.system("sudo shutdown now")

        time.sleep(LOOP_DELAY)

except KeyboardInterrupt:
    pass