Chapter 54: Bluetooth A2DP Source Implementation

#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <stdio.h>
#include <math.h> // For sin()

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/ringbuf.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "esp_bt.h"
#include "esp_bt_main.h"
#include "esp_bt_device.h"
#include "esp_gap_bt_api.h"
#include "esp_a2dp_api.h"
#include "esp_avrc_api.h" // For AVRCP Controller (optional)

static const char *TAG = "A2DP_SOURCE_DEMO";

// BD_ADDR of the A2DP Sink device to connect to (e.g., Bluetooth speaker)
// Replace with your Sink's BD_ADDR.
// Example: {0x11, 0x22, 0x33, 0x44, 0x55, 0x66}
// You can find this by scanning with another device or from the Sink's pairing info.
// For testing, you might need to discover it first or use a known paired device.
static esp_bd_addr_t s_peer_bda = {0xXX, 0xXX, 0xXX, 0xXX, 0xXX, 0xXX}; // TODO: REPLACE THIS!

// Audio properties for generated sine wave
#define SINE_WAVE_SAMPLE_RATE    44100
#define SINE_WAVE_CHANNELS       2 // Stereo
#define SINE_WAVE_BITS_PER_SAMPLE 16
#define SINE_WAVE_FREQUENCY      440 // A4 note
#define SINE_WAVE_AMPLITUDE      (INT16_MAX / 2) // Amplitude for 16-bit signed PCM

// Ring buffer for audio data
#define RINGBUFFER_SIZE (8 * 1024) // Bytes
static RingbufHandle_t s_audio_ringbuf = NULL;
static TaskHandle_t s_audio_gen_task_handle = NULL;

// A2DP application state machine
typedef enum {
    APP_AV_STATE_IDLE,
    APP_AV_STATE_DISCOVERING,
    APP_AV_STATE_DISCOVERED,
    APP_AV_STATE_UNCONNECTED,
    APP_AV_STATE_CONNECTING,
    APP_AV_STATE_CONNECTED,
    APP_AV_STATE_DISCONNECTING,
} app_av_state_t;

// Forward declarations
static void bt_app_gap_cb(esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *param);
static void bt_app_a2d_cb(esp_a2d_cb_event_t event, esp_a2d_cb_param_t *param);
static int32_t bt_app_a2d_data_cb(uint8_t *data, int32_t len); // Application provides data
static void audio_generator_task(void *arg);
static void bt_app_av_sm_handler(uint16_t event, void *param);

// Global state
static esp_a2d_connection_state_t s_conn_state = ESP_A2D_CONNECTION_STATE_DISCONNECTED;
static esp_a2d_audio_state_t s_audio_state = ESP_A2D_AUDIO_STATE_STOPPED;
static app_av_state_t s_app_av_state = APP_AV_STATE_IDLE;
static char s_local_bda_str[18];

// Helper function to convert Bluetooth Device Address (BDA) to string
static char *bda_to_str(const esp_bd_addr_t bda, char *str, size_t size) {
    if (bda == NULL || str == NULL || size < 18) {
        return NULL;
    }
    sprintf(str, "%02X:%02X:%02X:%02X:%02X:%02X",
            bda[0], bda[1], bda[2], bda[3], bda[4], bda[5]);
    return str;
}

void app_main(void) {
    esp_err_t ret;

    // --- Initialize NVS, Bluetooth Controller, Bluedroid Stack ---
    ESP_LOGI(TAG, "Initializing NVS...");
    ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK(ret);
    ESP_LOGI(TAG, "NVS initialized.");

    ESP_LOGI(TAG, "Releasing BT controller memory for BLE mode (if any)...");
    ESP_ERROR_CHECK(esp_bt_controller_mem_release(ESP_BT_MODE_BLE));

    ESP_LOGI(TAG, "Initializing Bluetooth controller...");
    esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT();
    ret = esp_bt_controller_init(&bt_cfg);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Initialize controller failed: %s", esp_err_to_name(ret));
        return;
    }
    ESP_LOGI(TAG, "Bluetooth controller initialized.");

    ESP_LOGI(TAG, "Enabling Bluetooth controller in Classic BT mode...");
    ret = esp_bt_controller_enable(ESP_BT_MODE_CLASSIC_BT);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Enable controller failed: %s", esp_err_to_name(ret));
        return;
    }
    ESP_LOGI(TAG, "Bluetooth controller enabled in Classic BT mode.");

    ESP_LOGI(TAG, "Initializing Bluedroid host stack...");
    ret = esp_bluedroid_init();
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Initialize Bluedroid failed: %s", esp_err_to_name(ret));
        return;
    }
    ESP_LOGI(TAG, "Bluedroid host stack initialized.");

    ESP_LOGI(TAG, "Enabling Bluedroid host stack...");
    ret = esp_bluedroid_enable();
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Enable Bluedroid failed: %s", esp_err_to_name(ret));
        return;
    }
    ESP_LOGI(TAG, "Bluedroid host stack enabled.");

    // --- Register Callbacks and Initialize A2DP Source ---
    ESP_LOGI(TAG, "Registering GAP callback...");
    ret = esp_bt_gap_register_callback(bt_app_gap_cb);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "GAP callback register failed: %s", esp_err_to_name(ret));
        return;
    }

    ESP_LOGI(TAG, "Registering A2DP Source event callback...");
    ret = esp_a2d_register_callback(bt_app_a2d_cb); // Common callback for Source & Sink events
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "A2DP event callback register failed: %s", esp_err_to_name(ret));
        return;
    }

    ESP_LOGI(TAG, "Registering A2DP Source data provider callback...");
    // This callback is invoked by the stack when it needs audio data from the application.
    ret = esp_a2d_source_register_data_callback(bt_app_a2d_data_cb);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "A2DP Source data callback register failed: %s", esp_err_to_name(ret));
        return;
    }

    ESP_LOGI(TAG, "Initializing A2DP Source profile...");
    ret = esp_a2d_source_init();
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "A2DP Source init failed: %s", esp_err_to_name(ret));
        return;
    }
    s_app_av_state = APP_AV_STATE_IDLE; // Initial state update after successful init

    // --- Set Device Name and Scan Mode ---
    const char * configured_device_name;
    esp_bt_dev_get_device_name(&configured_device_name);
    ESP_LOGI(TAG, "Device name is '%s' (from menuconfig).", configured_device_name);

    // ESP32 A2DP Source usually initiates connection, so discoverability might not be critical
    // But setting it can be useful for other profiles or debugging.
    ESP_LOGI(TAG, "Setting device to connectable and discoverable (optional for source)...");
    esp_bt_gap_set_scan_mode(ESP_BT_CONNECTABLE, ESP_BT_GENERAL_DISCOVERABLE);

    // --- Create Audio Ring Buffer and Generator Task ---
    s_audio_ringbuf = xRingbufferCreate(RINGBUFFER_SIZE, RINGBUF_TYPE_BYTEBUF);
    if (s_audio_ringbuf == NULL) {
        ESP_LOGE(TAG, "Failed to create audio ring buffer.");
        return;
    }
    ESP_LOGI(TAG, "Audio ring buffer created.");

    xTaskCreate(audio_generator_task, "AudioGenTask", 4096, NULL, configMAX_PRIORITIES - 3, &s_audio_gen_task_handle);
    ESP_LOGI(TAG, "Audio generator task created.");

    ESP_LOGI(TAG, "A2DP Source Demo Initialized. Will attempt to connect to Sink BDA: %s",
             bda_to_str(s_peer_bda, s_local_bda_str, sizeof(s_local_bda_str))); // Using s_local_bda_str as temp buffer for peer BDA

    // Get and log local Bluetooth device address
    const uint8_t *bda = esp_bt_dev_get_address();
    if (bda) {
        bda_to_str(bda, s_local_bda_str, sizeof(s_local_bda_str));
        ESP_LOGI(TAG, "Local Bluetooth Address: %s", s_local_bda_str);
    } else {
        ESP_LOGE(TAG, "Failed to get local Bluetooth address.");
    }
    
    // Automatically try to connect to the hardcoded peer BDA
    // Ensure s_peer_bda is correctly set above!
    // A check if s_peer_bda is all zeros might be good here.
    bool is_peer_bda_set = false;
    for(int i=0; i<ESP_BD_ADDR_LEN; ++i) {
        if(s_peer_bda[i] != 0x00) {
            is_peer_bda_set = true;
            break;
        }
    }
    if (!is_peer_bda_set) {
        ESP_LOGE(TAG, "Peer BDA is not set! Please update s_peer_bda in the code.");
        ESP_LOGW(TAG, "You can find the BDA of your speaker/headphones by pairing them with your phone/PC and checking the device details.");
    } else {
        ESP_LOGI(TAG, "Attempting to connect to A2DP Sink: %s", bda_to_str(s_peer_bda, (char[18]){0}, 18));
        esp_a2d_source_connect(s_peer_bda);
        s_app_av_state = APP_AV_STATE_CONNECTING;
    }
}

// Audio generator task: produces a sine wave and writes it to the ring buffer
static void audio_generator_task(void *arg) {
    uint32_t pcm_data_size = SINE_WAVE_CHANNELS * (SINE_WAVE_BITS_PER_SAMPLE / 8);
    int16_t *pcm_buf = malloc(1024 * pcm_data_size); // Buffer for ~1024 stereo samples
    if (!pcm_buf) {
        ESP_LOGE(TAG, "Failed to allocate PCM buffer for audio generator task.");
        vTaskDelete(NULL);
        return;
    }

    double phase = 0.0;
    double phase_step = 2.0 * M_PI * SINE_WAVE_FREQUENCY / SINE_WAVE_SAMPLE_RATE;

    ESP_LOGI(TAG, "Audio generator task started. Producing %d Hz sine wave at %d kHz, %d-bit, %s.",
             (int)SINE_WAVE_FREQUENCY, SINE_WAVE_SAMPLE_RATE / 1000, SINE_WAVE_BITS_PER_SAMPLE,
             SINE_WAVE_CHANNELS == 2 ? "stereo" : "mono");

    while (true) {
        if (s_audio_state == ESP_A2D_AUDIO_STATE_STARTED) {
            // Generate 1024 samples
            for (int i = 0; i < 1024; ++i) {
                int16_t sample_val = (int16_t)(SINE_WAVE_AMPLITUDE * sin(phase));
                if (SINE_WAVE_CHANNELS == 2) {
                    pcm_buf[i * 2] = sample_val;     // Left channel
                    pcm_buf[i * 2 + 1] = sample_val; // Right channel (same for simplicity)
                } else {
                    pcm_buf[i] = sample_val;         // Mono channel
                }
                phase += phase_step;
                if (phase >= 2.0 * M_PI) {
                    phase -= 2.0 * M_PI;
                }
            }
            // Send data to ring buffer
            // xRingbufferSend will block if buffer is full, up to a timeout (portMAX_DELAY here)
            UBaseType_t res = xRingbufferSend(s_audio_ringbuf, pcm_buf, 1024 * pcm_data_size, pdMS_TO_TICKS(100));
            if (res != pdTRUE) {
                ESP_LOGW(TAG, "Audio ring buffer send timeout or fail.");
            }
        } else {
            // Audio not started, wait a bit to avoid busy-looping
            vTaskDelay(pdMS_TO_TICKS(100));
        }
    }
    free(pcm_buf); // Should not be reached in this loop
    vTaskDelete(NULL);
}


// Bluetooth GAP callback
static void bt_app_gap_cb(esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *param) {
    char peer_bda_str[18];
    switch (event) {
        case ESP_BT_GAP_AUTH_CMPL_EVT:
            if (param->auth_cmpl.stat == ESP_BT_STATUS_SUCCESS) {
                ESP_LOGI(TAG, "GAP Authentication success. Device: %s, BDA: %s",
                         param->auth_cmpl.device_name,
                         bda_to_str(param->auth_cmpl.bda, peer_bda_str, sizeof(peer_bda_str)));
            } else {
                ESP_LOGE(TAG, "GAP Authentication failed. Status: %d, BDA: %s", param->auth_cmpl.stat,
                         bda_to_str(param->auth_cmpl.bda, peer_bda_str, sizeof(peer_bda_str)));
                // If auth fails during connection attempt, reset state
                if (s_app_av_state == APP_AV_STATE_CONNECTING && memcmp(param->auth_cmpl.bda, s_peer_bda, ESP_BD_ADDR_LEN) == 0) {
                    s_app_av_state = APP_AV_STATE_UNCONNECTED;
                }
            }
            break;
#if (CONFIG_BT_SSP_ENABLED == true)
        case ESP_BT_GAP_CFM_REQ_EVT:
            ESP_LOGI(TAG, "GAP SSP Confirmation request. BDA: %s. Numeric value: %d",
                     bda_to_str(param->cfm_req.bda, peer_bda_str, sizeof(peer_bda_str)), param->cfm_req.num_val);
            esp_bt_gap_ssp_confirm_reply(param->cfm_req.bda, true); // Auto-confirm
            break;
        // Other SSP events like KEY_NOTIF, KEY_REQ can be handled if needed
#endif
        default:
            ESP_LOGD(TAG, "Unhandled GAP Event: %d", event);
            break;
    }
}

// A2DP Source event callback (common for Source and Sink)
static void bt_app_a2d_cb(esp_a2d_cb_event_t event, esp_a2d_cb_param_t *param) {
    // Pass to state machine handler
    bt_app_av_sm_handler(event, param);
}

// A2DP Source data provider callback
// This function is called by the Bluedroid stack when it needs audio data.
// It should provide PCM data. The length `len` is the max bytes the stack can accept.
// Return value should be the number of bytes actually provided.
static int32_t bt_app_a2d_data_cb(uint8_t *data, int32_t len) {
    if (s_audio_ringbuf == NULL || len == 0 || data == NULL) {
        return 0;
    }

    size_t item_size = 0;
    // Attempt to receive data from the ring buffer
    // Non-blocking: 0 ticks timeout. If data is not immediately available, don't wait.
    uint8_t *buffer_item = (uint8_t *)xRingbufferReceiveUpTo(s_audio_ringbuf, &item_size, 0, len);

    if (buffer_item != NULL && item_size > 0) {
        memcpy(data, buffer_item, item_size);
        // Return the received item to the ring buffer
        vRingbufferReturnItem(s_audio_ringbuf, (void *)buffer_item);
        return item_size;
    }
    // No data available or error
    return 0;
}

// Application state machine handler for A2DP Source
static void bt_app_av_sm_handler(uint16_t event, void *param) {
    ESP_LOGD(TAG, "%s: Event 0x%x, Current AV State: %d, Conn State: %d, Audio State: %d",
             __func__, event, s_app_av_state, s_conn_state, s_audio_state);
    char peer_bda_str[18];

    switch (event) {
        case ESP_A2D_CONNECTION_STATE_EVT: {
            esp_a2d_cb_param_t *a2d_param = (esp_a2d_cb_param_t *)(param);
            s_conn_state = a2d_param->conn_stat.state; // Update global connection state
            bda_to_str(a2d_param->conn_stat.remote_bda, peer_bda_str, sizeof(peer_bda_str));
            ESP_LOGI(TAG, "A2DP Connection State: %s, BDA: %s",
                     s_conn_state == ESP_A2D_CONNECTION_STATE_DISCONNECTED ? "Disconnected" :
                     s_conn_state == ESP_A2D_CONNECTION_STATE_CONNECTING   ? "Connecting" :
                     s_conn_state == ESP_A2D_CONNECTION_STATE_CONNECTED    ? "Connected" : "Unknown",
                     peer_bda_str);

            if (s_conn_state == ESP_A2D_CONNECTION_STATE_CONNECTED) {
                s_app_av_state = APP_AV_STATE_CONNECTED;
                ESP_LOGI(TAG, "Connected to Sink. Ready to start audio stream.");
                // Optionally, auto-start the stream after connection
                // esp_a2d_media_ctrl(ESP_A2D_MEDIA_CTRL_START); // Or wait for user action
            } else if (s_conn_state == ESP_A2D_CONNECTION_STATE_DISCONNECTED) {
                s_app_av_state = APP_AV_STATE_UNCONNECTED;
                s_audio_state = ESP_A2D_AUDIO_STATE_STOPPED; // Ensure audio state is reset
                ESP_LOGI(TAG, "Disconnected from Sink.");
                // Re-attempt connection or wait for user action
                // For simplicity, this example doesn't auto-reconnect here.
            } else if (s_conn_state == ESP_A2D_CONNECTION_STATE_CONNECTING) {
                s_app_av_state = APP_AV_STATE_CONNECTING;
            }
            break;
        }
        case ESP_A2D_AUDIO_STATE_EVT: {
            esp_a2d_cb_param_t *a2d_param = (esp_a2d_cb_param_t *)(param);
            s_audio_state = a2d_param->audio_stat.state; // Update global audio state
            ESP_LOGI(TAG, "A2DP Audio State: %s",
                     s_audio_state == ESP_A2D_AUDIO_STATE_STARTED ? "Started" :
                     s_audio_state == ESP_A2D_AUDIO_STATE_STOPPED ? "Stopped" :
                     s_audio_state == ESP_A2D_AUDIO_STATE_REMOTE_SUSPEND ? "Remote Suspend" : "Unknown");

            if (s_audio_state == ESP_A2D_AUDIO_STATE_STARTED) {
                ESP_LOGI(TAG, "Audio stream started. Generator task will now actively send data.");
                // Audio generator task will see s_audio_state and start pushing to ringbuffer
            } else if (s_audio_state == ESP_A2D_AUDIO_STATE_STOPPED || s_audio_state == ESP_A2D_AUDIO_STATE_REMOTE_SUSPEND) {
                ESP_LOGI(TAG, "Audio stream stopped or suspended. Generator task will pause sending.");
                // Clear ring buffer if needed, or let it drain if stream might resume.
                // For simplicity, we just let the generator pause.
                // If stopped and not planning to resume soon, could clear ringbuffer:
                // if (s_audio_ringbuf) { xRingbufferReset(s_audio_ringbuf); }
            }
            break;
        }
        case ESP_A2D_AUDIO_CFG_EVT: { // Audio stream configuration event
            esp_a2d_cb_param_t *a2d_param = (esp_a2d_cb_param_t *)(param);
            ESP_LOGI(TAG, "A2DP Audio Configured by Sink: Codec: %s, Sample Rate: %d Hz, Channels: %d, Bits/Sample: %d",
                     (a2d_param->audio_cfg.mcc.type == ESP_A2D_MCT_SBC) ? "SBC" : "Other",
                     a2d_param->audio_cfg.mcc.sample_freq,
                     a2d_param->audio_cfg.mcc.channels,
                     a2d_param->audio_cfg.mcc.bits_per_sample);
            // This confirms the parameters the Sink has accepted.
            // Our generated sine wave uses fixed parameters, so we hope they match or are compatible.
            // The A2DP stack handles the negotiation.
            // If we are connected and configured, we can start the media stream.
            if (s_conn_state == ESP_A2D_CONNECTION_STATE_CONNECTED && s_audio_state == ESP_A2D_AUDIO_STATE_STOPPED) {
                 ESP_LOGI(TAG, "Audio configured. Sending MEDIA_CTRL_START to begin streaming.");
                 esp_a2d_media_ctrl(ESP_A2D_MEDIA_CTRL_START);
            }
            break;
        }
        case ESP_A2D_MEDIA_CTRL_ACK_EVT: { // Acknowledgement for media control command
            esp_a2d_cb_param_t *a2d_param = (esp_a2d_cb_param_t *)(param);
            ESP_LOGI(TAG, "A2DP Media Control ACK: Command %s, Status %s",
                     a2d_param->media_ctrl_stat.cmd == ESP_A2D_MEDIA_CTRL_START ? "START" :
                     a2d_param->media_ctrl_stat.cmd == ESP_A2D_MEDIA_CTRL_STOP  ? "STOP"  : "UNKNOWN_CMD",
                     a2d_param->media_ctrl_stat.status == ESP_A2D_MEDIA_CTRL_ACK_SUCCESS ? "SUCCESS" : "FAILURE");

            if (a2d_param->media_ctrl_stat.cmd == ESP_A2D_MEDIA_CTRL_START &&
                a2d_param->media_ctrl_stat.status == ESP_A2D_MEDIA_CTRL_ACK_SUCCESS) {
                ESP_LOGI(TAG, "Media START acknowledged by Sink. Audio should begin playing on Sink.");
                // Audio state will change via ESP_A2D_AUDIO_STATE_EVT
            } else if (a2d_param->media_ctrl_stat.cmd == ESP_A2D_MEDIA_CTRL_STOP &&
                       a2d_param->media_ctrl_stat.status == ESP_A2D_MEDIA_CTRL_ACK_SUCCESS) {
                ESP_LOGI(TAG, "Media STOP acknowledged by Sink.");
                 s_audio_state = ESP_A2D_AUDIO_STATE_STOPPED; // Manually update as audio state event might be delayed
            }
            break;
        }
        // Other events like ESP_A2D_PROF_STATE_EVT can be handled if needed for A2DP source deinit.
        default:
            ESP_LOGD(TAG, "Unhandled A2DP Event: 0x%x", event);
            break;
    }
}