from micropython import const
import asyncio
# import aioble
# import bluetooth
import utime
import math
from machine import Pin, PWM

# # ─── BLE MIDI UUIDs ───────────────────────────────────────────────────────────
# _MIDI_SERVICE_UUID = bluetooth.UUID("03B80E5A-EDE8-4B33-A751-6CE34EC4C700")
# _MIDI_CHAR_UUID    = bluetooth.UUID("7772E5DB-3868-4112-A1A9-F2669D106BF3")

# ─── Hardware ─────────────────────────────────────────────────────────────────
buzzer1 = PWM(Pin(15))
buzzer2 = PWM(Pin(14))
buzzer1.duty_u16(0)
buzzer2.duty_u16(0)

# ─── MIDI note → frequency  (A4 = 440 Hz, MIDI 69) ───────────────────────────
def midi_to_freq(note: int) -> int:
    return int(440 * math.pow(2, (note - 69) / 12))

# ─── Button → MIDI note number mapping  (expand as needed) ───────────────────
#   C4=60, D4=62, E4=64, F4=65, G4=67, A4=69, B4=71, C5=72
BUTTON_NOTE_MAP = {
    Pin(0,  Pin.IN, Pin.PULL_UP): 60,   # C4
    Pin(1,  Pin.IN, Pin.PULL_UP): 62,   # D4
    Pin(2,  Pin.IN, Pin.PULL_UP): 64,   # E4
    Pin(3,  Pin.IN, Pin.PULL_UP): 65,   # F4
    Pin(4,  Pin.IN, Pin.PULL_UP): 67,   # G4
    Pin(5,  Pin.IN, Pin.PULL_UP): 69,   # A4
    Pin(6,  Pin.IN, Pin.PULL_UP): 71,   # B4
    Pin(7,  Pin.IN, Pin.PULL_UP): 72,   # C5
}

# ─── Diatonic third (harmony) ─────────────────────────────────────────────────
# A "third above" in a key is NOT a fixed semitone count: C->E is 4 semitones
# (major third) but D->F is 3 (minor third). To stay in key we snap the note to
# its C-major scale degree, step up two degrees, and convert back to a MIDI note.
#
# C-major pitch classes (semitone offsets from C): 0 2 4 5 7 9 11
_C_MAJOR = (0, 2, 4, 5, 7, 9, 11)

def third_above(note: int) -> int:
    """Return the MIDI note a diatonic third above `note` in C major.
    If the note isn't in C major, fall back to a +4 semitone major third."""
    octave, pc = note // 12, note % 12
    if pc not in _C_MAJOR:
        return note + 4
    degree = _C_MAJOR.index(pc)          # 0..6
    new_degree = degree + 2              # up a third = two scale steps
    octave += new_degree // 7            # wrap past the 7th degree into next octave
    new_pc = _C_MAJOR[new_degree % 7]
    result = octave * 12 + new_pc
    return result if result <= 127 else note   # never exceed MIDI range

# ─── State ────────────────────────────────────────────────────────────────────
_connection = None      # active aioble Connection, or None when disconnected
_midi_char  = None      # the MIDI Characteristic, set up in ble_task()

# ─── BLE MIDI packet builder ──────────────────────────────────────────────────
# Spec: [header, timestamp_low, status, data1, data2]
#   header        = 0x80 | (ms >> 7) & 0x3F
#   timestamp_low = 0x80 | (ms & 0x7F)
def _midi_packet(status: int, data1: int, data2: int) -> bytes:
    ms = utime.ticks_ms() & 0x1FFF
    header        = 0x80 | ((ms >> 7) & 0x3F)
    timestamp_low = 0x80 | (ms & 0x7F)
    return bytes([header, timestamp_low, status, data1, data2])

def _note_on(note: int, velocity: int = 100) -> bytes:
    return _midi_packet(0x90, note, velocity)

def _note_off(note: int) -> bytes:
    return _midi_packet(0x80, note, 0)

# ─── MIDI send helpers ────────────────────────────────────────────────────────
def _ble_notify(packet: bytes) -> None:
    """Send a BLE MIDI notification if connected."""
    if _connection and _midi_char:
        try:
            _midi_char.notify(_connection, packet)
        except Exception as e:
            print("notify error:", e)

async def play_note(midi_note: int, duration_ms: int = 200) -> None:
    """Play a note plus its diatonic third in harmony, on both buzzers,
    and broadcast both notes over BLE."""
    harmony_note = third_above(midi_note)

    # Note On — both voices start together
    _ble_notify(_note_on(midi_note))
    _ble_notify(_note_on(harmony_note))
    buzzer1.freq(midi_to_freq(midi_note))
    buzzer1.duty_u16(32768)
    buzzer2.freq(midi_to_freq(harmony_note))
    buzzer2.duty_u16(32768)

    await asyncio.sleep_ms(duration_ms)

    # Note Off — both voices stop together
    _ble_notify(_note_off(midi_note))
    _ble_notify(_note_off(harmony_note))
    buzzer1.duty_u16(0)
    buzzer2.duty_u16(0)

# ─── BLE task ─────────────────────────────────────────────────────────────────
async def ble_task() -> None:
    global _connection, _midi_char

    midi_service = aioble.Service(_MIDI_SERVICE_UUID)
    _midi_char = aioble.Characteristic(
        midi_service,
        _MIDI_CHAR_UUID,
        read=True,
        write=True,
        notify=True,
        capture=True,
    )
    aioble.register_services(midi_service)

    while True:
        try:
            print("Advertising…")
            conn = await aioble.advertise(
                100_000,                        # 100 ms interval (was 250 ms — too slow)
                name="PicoInstrument",
                services=[_MIDI_SERVICE_UUID],
            )
            print("Connected:", conn.device)
            _connection = conn

            async with conn:
                await conn.disconnected()       # wait until host drops

            print("Disconnected")
        except Exception as e:
            print("BLE error:", e)
        finally:
            _connection = None                  # clear on disconnect so stale notifies don't fire
            await asyncio.sleep_ms(500)

# ─── Button scanner ───────────────────────────────────────────────────────────
async def button_task() -> None:
    # Track previous state for each pin so we can detect edges (press, not hold)
    prev = {pin: True for pin in BUTTON_NOTE_MAP}   # PULL_UP → idle = HIGH (True)

    while True:
        for pin, note in BUTTON_NOTE_MAP.items():
            current = pin.value()
            if prev[pin] and not current:           # falling edge = press
                await play_note(note)
                await asyncio.sleep_ms(50)          # debounce after note finishes
            prev[pin] = current
        await asyncio.sleep_ms(10)

# ─── Entry point ──────────────────────────────────────────────────────────────
async def main() -> None:
    # asyncio.create_task(ble_task())
    await button_task()

asyncio.run(main())