import board
import time
import neopixel
from circuitPyHuskyLib import HuskyLensLibrary
from adafruit_led_animation.color import RED, ORANGE, YELLOW, GREEN, TEAL, BLUE, PINK, WHITE, BLACK

# Initialize NeoPixel strip
strip = neopixel.NeoPixel(board.GP10, 60, brightness=0.75, auto_write=False)

# Initialize HuskyLens
hl = HuskyLensLibrary('UART', TX=board.GP8, RX=board.GP9)

# Start with Face Recognition Algorithm
hl.algorithm("ALGORITHM_FACE_RECOGNITION")
print("Face Recognition Algorithm initialized.")

# Initialize the active flag
active = False

# Colors (r,g,b)
OFF = (0, 0, 0)
MOVING_BLUE = BLUE
WHITE = (255, 255, 255)

# Assign different colors to IDs for color recognition
color = {
    1: RED,
    2: ORANGE,
    3: YELLOW,
    4: GREEN,
    5: TEAL,
    6: BLUE,
    7: PINK
}

def smooth_fade(current_color, target_color, steps=50, delay=0.01):
    """Smoothly fades between two colors."""
    for step in range(steps + 1):
        intermediate_color = tuple(
            int(current_color[i] + (target_color[i] - current_color[i]) * step / steps)
            for i in range(3)
        )
        strip.fill(intermediate_color)
        strip.show()
        time.sleep(delay)

def dim_color(color, factor=0.5):
    """Dims a color by the specified factor."""
    return tuple(int(c * factor) for c in color)

def moving_blue_animation():
    """Moves a single blue LED along the strip."""
    for i in range(60):
        strip.fill(OFF)  # Turn off all LEDs
        strip[i] = MOVING_BLUE  # Turn on the moving LED
        strip.show()
        time.sleep(0.05)

last_color = OFF  # Keep track of the last detected color

while True:
    try:
        if not active:  # Check for face recognition only when inactive
            print("\n[STATUS] Face Recognition is running...")
            results = hl.learnedBlocks()  # Get face recognition results

            if results:  # If any faces are recognized
                all_id = list(set([result.ID for result in results]))
                all_id.sort()
                print(f"[INFO] Detected Face IDs: {all_id}")

                if 1 in all_id:  # If ID 1 is recognized
                    active = True  # Activate the rest of the code
                    print("[SUCCESS] Face ID 1 recognized! Activating Color Recognition...")
                    # Fade into white
                    smooth_fade(OFF, WHITE, steps=100, delay=0.02)
                    # Switch to color recognition algorithm
                    hl.algorithm("ALGORITHM_COLOR_RECOGNITION")
                    print("Color Recognition Algorithm initialized.")
                else:
                    print("[INFO] Face ID 1 not recognized. Running moving blue animation.")
                    moving_blue_animation()  # Show moving blue LED animation
            else:
                print("[INFO] No faces detected. Running moving blue animation.")
                moving_blue_animation()  # Show moving blue LED animation

        else:  # If active, perform color recognition
            print("\n[STATUS] Color Recognition is running...")
            results = hl.learnedBlocks()  # Get color recognition results

            if results:  # If any colors are recognized
                all_id = list(set([result.ID for result in results]))  # Get all color IDs
                all_id.sort()
                print(f"[INFO] Detected Color IDs: {all_id}")

                if len(all_id) > 3:  # Distribute multiple colors along the strip
                    detected_colors = [color.get(id, BLACK) for id in all_id]
                    print(f"[ACTION] Creating gradient for colors: {detected_colors}")
                    create_gradient(detected_colors, 60)
                    last_color = detected_colors[-1]
                else:
                    first = all_id[0]  # Use the first detected color ID
                    print(f"[ACTION] Setting color for ID: {first}")
                    current_color = strip[0]  # Get the current strip color
                    target_color = color.get(first, BLACK)
                    smooth_fade(current_color, target_color, steps=100, delay=0.01)
                    last_color = target_color  # Update the last color
            else:
                print("[INFO] No colors detected. Dimming to half brightness.")
                dimmed_color = dim_color(last_color, factor=0.5)
                smooth_fade(strip[0], dimmed_color, steps=100, delay=0.01)

    except KeyError:  # Handle undefined color IDs
        print("[ERROR] Undefined Color ID detected. Setting strip to WHITE.")
        smooth_fade(strip[0], WHITE, steps=100, delay=0.01)

    finally:
        time.sleep(0.5)