import sys
import time
from machine import Pin, PWM, time_pulse_us

# ============================================================
# ПРИНУДИТЕЛЬНЫЙ ВЫВОД В КОНСОЛЬ
# ============================================================
print("=" * 50)
print("КОД ЗАПУЩЕН! Если вы это видите - консоль работает")
print("=" * 50)

# ============================================================
# НАСТРОЙКА АППАРАТНОЙ ЧАСТИ
# ============================================================
# Определяем пины согласно схеме
LED_PIN = 15        # Индикаторный светодиод
DHT_PIN = 27        # Датчик DHT22

# Пины для эхолокационного радара
TRIG_PIN = 5        # HC-SR04 Trigger
ECHO_PIN = 18       # HC-SR04 Echo
SERVO_PIN = 13      # Сервопривод для вращения радара

# Инициализация
led = Pin(LED_PIN, Pin.OUT)
trig = Pin(TRIG_PIN, Pin.OUT)   # ВАЖНО: TRIG должен быть OUTPUT!
echo = Pin(ECHO_PIN, Pin.IN)
servo = PWM(Pin(SERVO_PIN), freq=50)

print("[OK] Пины инициализированы")
print(f"  LED на GPIO{LED_PIN}")
print(f"  TRIG на GPIO{TRIG_PIN}")
print(f"  ECHO на GPIO{ECHO_PIN}")
print(f"  SERVO на GPIO{SERVO_PIN}")

# ============================================================
# ФУНКЦИЯ ДЛЯ СЕРВОПРИВОДА
# ============================================================
def set_angle(angle):
    """Устанавливает угол сервопривода (0-180 градусов)"""
    duty = int(26 + (angle / 180) * 102)
    servo.duty(duty)
    time.sleep_ms(50)

# ============================================================
# ФУНКЦИЯ ДЛЯ УЛЬТРАЗВУКОВОГО ДАТЧИКА
# ============================================================
def get_distance():
    """Измеряет расстояние в см с помощью HC-SR04"""
    # Отправляем импульс
    trig.value(0)
    time.sleep_us(2)
    trig.value(1)
    time.sleep_us(10)
    trig.value(0)
    
    # Измеряем время ответа
    duration = time_pulse_us(echo, 1, 30000)
    
    if duration < 0:
        return 500  # Ошибка или слишком далеко
    
    # Расстояние в см (скорость звука 343 м/с = 0.0343 см/мкс)
    distance = duration * 0.0343 / 2
    return distance

# ============================================================
# СТАРТОВОЕ МИГАНИЕ LED
# ============================================================
print("\n[TEST] Стартовое мигание LED...")
for i in range(3):
    led.value(1)
    print(f"  LED ON {i+1}")
    time.sleep(0.2)
    led.value(0)
    print(f"  LED OFF {i+1}")
    time.sleep(0.2)

# ============================================================
# ТЕСТ СЕРВОПРИВОДА
# ============================================================
print("\n[TEST] Тест сервопривода...")
try:
    set_angle(0)
    print("  Угол 0° - OK")
    time.sleep(1)
    set_angle(90)
    print("  Угол 90° - OK")
    time.sleep(1)
    set_angle(180)
    print("  Угол 180° - OK")
    time.sleep(1)
    set_angle(0)
    print("  Угол 0° - OK")
except Exception as e:
    print(f"  Ошибка серво: {e}")

# ============================================================
# ТЕСТ УЛЬТРАЗВУКОВОГО ДАТЧИКА
# ============================================================
print("\n[TEST] Тест УЗ-датчика...")
for i in range(3):
    dist = get_distance()
    print(f"  Измерение {i+1}: {dist:.0f} см")
    time.sleep(0.5)

# ============================================================
# ОСНОВНОЙ ЦИКЛ
# ============================================================
print("\n" + "=" * 50)
print("ЗАПУСК ОСНОВНОГО ЦИКЛА")
print("Сканирование будет каждые 5 секунд")
print("=" * 50)

cycle = 0
last_scan = time.time()
SCAN_INTERVAL = 5

while True:
    current_time = time.time()
    
    if current_time - last_scan >= SCAN_INTERVAL:
        cycle += 1
        print(f"\n--- СКАНИРОВАНИЕ #{cycle} ---")
        
        # Включаем LED
        led.value(1)
        
        # Сканируем 3 сектора для теста
        distances = []
        sectors = [0, 60, 120, 180]
        
        for angle in sectors:
            print(f"  Поворот на {angle}°...")
            set_angle(angle)
            time.sleep(0.3)
            
            dist = get_distance()
            distances.append(dist)
            print(f"    Расстояние: {dist:.0f} см")
        
        # Подсчёт людей (упрощённый)
        people = 0
        for d in distances:
            if 40 < d < 400:
                people += 1
        
        print(f"  [РЕЗУЛЬТАТ] Людей в комнате: {people}")
        
        # Отправка JSON
        json_data = '{"cycle":' + str(cycle) + ',"people":' + str(people) + ',"distances":' + str(distances) + '}'
        print(f"  [JSON] {json_data}")
        
        # Выключаем LED
        led.value(0)
        
        last_scan = current_time
    
    time.sleep(0.1)