import board
import neopixel
import touchio
import time
import math
import digitalio
import simpleio
import busio
import adafruit_lis3dh
import random
from adafruit_bluefruit_connect.packet import Packet
from adafruit_bluefruit_connect.color_packet import ColorPacket
import _bleio
import adafruit_ble
from adafruit_ble.advertising.standard import ProvideServicesAdvertisement
from adafruit_ble.services.nordic import UARTService

# Define major colors
COLORS = [
    (255, 0, 0),    # Red
    (255, 127, 0),  # Orange
    (255, 255, 0),  # Yellow
    (127, 255, 0),  # Chartreuse
    (0, 255, 0),    # Green
    (0, 255, 127),  # Spring Green
    (0, 255, 255),  # Cyan
    (0, 127, 255),  # Azure
    (0, 0, 255),    # Blue
    (127, 0, 255),  # Violet
    (255, 0, 255),  # Magenta
    (255, 0, 127),  # Rose
]

# Default blue color
DEFAULT_BLUE = (0, 100, 255)

# BLE setup
ble = adafruit_ble.BLERadio()
uart = UARTService()
advertisement = ProvideServicesAdvertisement(uart)

print("WAITING FOR BLUETOOTH CONNECTION...")
ble.start_advertising(advertisement)

# Add accelerometer setup
i2c = busio.I2C(board.ACCELEROMETER_SCL, board.ACCELEROMETER_SDA)
int1 = digitalio.DigitalInOut(board.ACCELEROMETER_INTERRUPT)
sensor = adafruit_lis3dh.LIS3DH_I2C(i2c, address=0x19, int1=int1)
sensor.range = adafruit_lis3dh.RANGE_8_G

# Define states as constants
STATE_OFF = 0
STATE_POWERING_ON = 1
STATE_IDLE = 2
STATE_POWERING_OFF = 3

# Basic hardware setup with error handling
try:
    PIXEL_PIN = board.A2
    PIXEL_COUNT = 30
    pixels = neopixel.NeoPixel(PIXEL_PIN, PIXEL_COUNT, brightness=0.5, auto_write=False)
    print("NeoPixel setup successful")
except Exception as e:
    print("Failed to initialize NeoPixels!")
    raise

# Touch setup with error handling
try:
    touch_pad = touchio.TouchIn(board.A1)
    print("Touch pad setup successful")
except Exception as e:
    print("Failed to initialize touch pad!")
    raise

# Additional touch pad for color cycling
color_touch = touchio.TouchIn(board.A5)

# Speaker setup
speaker_enable = digitalio.DigitalInOut(board.SPEAKER_ENABLE)
speaker_enable.direction = digitalio.Direction.OUTPUT
speaker_enable.value = True

# Colors
OFF_COLOR = (0, 0, 0)
BLADE_COLOR = (0, 100, 255)  # Base blue color

def generate_pulse_colors(base_color):
    """Generate variations of the base color plus white highlight"""
    r, g, b = base_color
    darker = (int(r * 0.8), int(g * 0.8), int(b * 0.8))
    brighter = (min(255, int(r * 1.2)), min(255, int(g * 1.2)), min(255, int(b * 1.2)))
    highlight = (min(255, int(r + 200)), min(255, int(g + 200)), min(255, int(b + 200)))
    return [darker, base_color, brighter, highlight]

PULSE_COLORS = generate_pulse_colors(BLADE_COLOR)

class Sword:
    LIGHT_SPEED = 0.01
    WAVE_SPEED = 1.0
    SWING_THRESHOLD = 15.0  # Adjust this value based on testing
    SWING_COOLDOWN = 0.3    # Minimum time between swing sounds
    
    def __init__(self):
        self.state = STATE_OFF
        self.pixels = pixels
        self.touch_sensor = touch_pad
        self.color_touch = color_touch
        self.pulse_start_time = time.monotonic()
        self.last_sensor_print = 0
        self.last_swing_time = 0
        self.last_color_touch_time = 0
        self.color_index = 0  # For cycling through colors
        self.blade_color = DEFAULT_BLUE
        self.ble_connected = False
        print("Sword initialized successfully")
        self.pixels.fill(OFF_COLOR)
        self.pixels.show()
    
    def check_ble(self):
        """Check for BLE color commands"""
        if not ble.connected:
            self.ble_connected = False
            if not ble.advertising:
                print("Starting BLE advertising...")
                ble.start_advertising(advertisement)
            return

        self.ble_connected = True
        if uart.in_waiting:
            try:
                packet = Packet.from_stream(uart)
                if isinstance(packet, ColorPacket):
                    self.blade_color = packet.color
                    # Update pulse colors with new base color
                    global PULSE_COLORS
                    PULSE_COLORS = generate_pulse_colors(self.blade_color)
                    print(f"Color updated to: RGB{self.blade_color}")
            except Exception as e:
                print(f"Error processing packet: {e}")
    
    def check_color_touch(self):
        """Handle color cycling touch pad"""
        current_time = time.monotonic()
        if self.color_touch.value and current_time - self.last_color_touch_time > 0.2:  # Debounce
            self.color_index = (self.color_index + 1) % len(COLORS)
            self.blade_color = COLORS[self.color_index]
            # Update pulse colors with new base color
            global PULSE_COLORS
            PULSE_COLORS = generate_pulse_colors(self.blade_color)
            print(f"Color cycled to: RGB{self.blade_color}")
            self.last_color_touch_time = current_time
    
    def play_swing_sound(self):
        """Play a random swing sound effect"""
        swing_number = random.randint(1, 4)
        if swing_number == 1:
            frequencies = [800, 800, 600, 600, 400, 400]  # Quick downward swish
        elif swing_number == 2:
            frequencies = [400, 400, 600, 600, 800, 800]  # Quick upward swish
        elif swing_number == 3:
            frequencies = [600, 600, 800, 800, 800, 600, 600]  # Arc swing
        else:
            frequencies = [800, 800, 400, 400, 400, 800, 800]  # Circular swing
            
        for freq in frequencies:
            simpleio.tone(board.SPEAKER, freq, duration=0.02)
            time.sleep(0.01)
    
    def check_for_swing(self):
        """Check if the sword is being swung"""
        current_time = time.monotonic()
        
        # Only check for swings if enough time has passed since the last one
        if current_time - self.last_swing_time >= self.SWING_COOLDOWN:
            x, y, z = sensor.acceleration
            # Calculate the magnitude of acceleration
            magnitude = math.sqrt(x*x + y*y + z*z)
            
            # If magnitude exceeds threshold, it's a swing
            if magnitude > self.SWING_THRESHOLD:
                print(f"Swing detected! Magnitude: {magnitude:.2f}")
                self.play_swing_sound()
                self.last_swing_time = current_time
    
    def print_sensor_values(self):
        """Print accelerometer values every 0.5 seconds"""
        current_time = time.monotonic()
        if current_time - self.last_sensor_print >= 0.5:
            x, y, z = sensor.acceleration
            print(f"Acceleration: X:{x:6.2f}, Y:{y:6.2f}, Z:{z:6.2f} m/s^2")
            print("-" * 50)
            
            self.last_sensor_print = current_time
    
    def power_on_tone(self):
        """Rising pitch for power on"""
        frequencies = [300, 400, 500, 600, 700, 800]
        for freq in frequencies:
            simpleio.tone(board.SPEAKER, freq, duration=0.05)
            time.sleep(0.02)
    
    def power_off_tone(self):
        """Falling pitch for power off"""
        frequencies = [800, 700, 600, 500, 400, 300]
        for freq in frequencies:
            simpleio.tone(board.SPEAKER, freq, duration=0.05)
            time.sleep(0.02)
    
    def update(self):
        """Main state machine update method"""
        try:
            self.print_sensor_values()
            self.check_ble()  # Check for BLE updates
            
            # If not connected to BLE and blade color isn't default, reset it
            if not self.ble_connected and self.blade_color != DEFAULT_BLUE:
                self.blade_color = DEFAULT_BLUE
                global PULSE_COLORS
                PULSE_COLORS = generate_pulse_colors(self.blade_color)
            
            # Check color touch pad regardless of BLE connection
            self.check_color_touch()
            
            if not self.touch_sensor.value and self.state != STATE_OFF:
                self.state = STATE_POWERING_OFF
                self.power_off()
                return

            if self.state == STATE_OFF:
                if self.touch_sensor.value:
                    self.state = STATE_POWERING_ON
                    self.power_on()
                    
            elif self.state == STATE_POWERING_ON:
                self.state = STATE_IDLE
                    
            elif self.state == STATE_IDLE:
                self.wave_blade()
                self.check_for_swing()
                    
        except Exception as e:
            print("Error in update method:", e)
    
    def power_on(self):
        """Power on animation with rising tone"""
        self.power_on_tone()
        self.pixels.brightness = 0.5
        middle = PIXEL_COUNT // 2
        
        for i in range(middle + 1):
            if i < PIXEL_COUNT:
                self.pixels[i] = self.blade_color  # Use current blade color
            tip_pixel = PIXEL_COUNT - 1 - i
            if tip_pixel >= 0:
                self.pixels[tip_pixel] = self.blade_color  # Use current blade color
            self.pixels.show()
            time.sleep(self.LIGHT_SPEED)
    
    def power_off(self):
        """Power off animation with falling tone"""
        self.power_off_tone()
        self.pixels.brightness = 0.5
        middle = PIXEL_COUNT // 2
        
        for i in range(middle + 1):
            if middle + i < PIXEL_COUNT:
                self.pixels[middle + i] = OFF_COLOR
            if middle - i >= 0:
                self.pixels[middle - i] = OFF_COLOR
            self.pixels.show()
            time.sleep(self.LIGHT_SPEED)
            
        self.state = STATE_OFF
    
    def wave_blade(self):
        """Create wave effect on blade"""
        try:
            current_time = time.monotonic()
            base_brightness = 0.5 + (math.sin(current_time * 1.5) * 0.15)
            self.pixels.brightness = base_brightness
            
            for i in range(PIXEL_COUNT):
                position_offset = i / (PIXEL_COUNT * 1.5) * math.pi * 2
                time_offset = current_time * self.WAVE_SPEED
                wave = math.sin(position_offset + time_offset)
                
                color_index = int((wave + 1) * (len(PULSE_COLORS) - 1) / 2)
                color_index = max(0, min(color_index, len(PULSE_COLORS) - 2))
                mix = (wave + 1) * (len(PULSE_COLORS) - 1) / 2 - color_index
                
                color1 = PULSE_COLORS[color_index]
                color2 = PULSE_COLORS[color_index + 1]
                
                r = int(color1[0] * (1 - mix) + color2[0] * mix)
                g = int(color1[1] * (1 - mix) + color2[1] * mix)
                b = int(color1[2] * (1 - mix) + color2[2] * mix)
                
                self.pixels[i] = (r, g, b)
            
            self.pixels.show()
            
        except Exception as e:
            print("Error in wave_blade method:", e)

# Main loop with error handling
try:
    print("Creating sword instance...")
    sword = Sword()
    print("Entering main loop...")
    
    while True:
        sword.update()
        time.sleep(0.01)
            
except Exception as e:
    print("Fatal error in main loop:", e)
    while True:
        try:
            pixels.fill((255, 0, 0))
            pixels.show()
            time.sleep(0.5)
            pixels.fill((0, 0, 0))
            pixels.show()
            time.sleep(0.5)
        except:
            pass