from codrone_edu.drone import *
import keyboard
import time

drone = Drone()
drone.connect()      # or drone.pair() depending on your setup

print("Connecting to drone...")
time.sleep(3)        # Give it time to connect

try:
    drone.takeoff()
    print("Drone took off! Use Arrow Keys to fly.")
    print("↑ Forward | ↓ Backward | ← Left | → Right")
    print("Hold keys for continuous movement. Press 'q' to land and quit.")

    power = 25        # ← Keep this LOW for safety (15-30 recommended)

    while True:
        # Reset all movement every loop
        drone.set_roll(0)
        drone.set_pitch(0)
        drone.set_yaw(0)
        drone.set_throttle(0)

        # Arrow key controls (slow & safe)
        if keyboard.is_pressed('up'):
            drone.set_pitch(power)      # Forward
        if keyboard.is_pressed('down'):
            drone.set_pitch(-power)     # Backward
        if keyboard.is_pressed('left'):
            drone.set_roll(-power)      # Left
        if keyboard.is_pressed('right'):
            drone.set_roll(power)       # Right

        # Optional: Add throttle control with other keys
        if keyboard.is_pressed('w'):
            drone.set_throttle(power)   # Up
        if keyboard.is_pressed('s'):
            drone.set_throttle(-power)  # Down

        if keyboard.is_pressed('q'):
            print("Landing...")
            break

        drone.move(0.1)   # Send command every 0.1 seconds (smooth control)
        time.sleep(0.05)  # Fast loop for responsive feel

except Exception as e:
    print("Error:", e)

finally:
    drone.land()
    drone.disconnect()
    print("Drone landed and disconnected.")