#include "k230_audio.h"
#include <stdexcept>
#include <cstring>
#include <iostream>
#include <fcntl.h>
#include <unistd.h>
#include <sys/mman.h>
#include <errno.h>

#include <sys/ioctl.h>
#include "mpi_vb_api.h"
#include "mpi_sys_api.h"
#include "mpi_ai_api.h"
#include "mpi_ao_api.h"
#include "drv_fpioa.h"

// Constant definitions
#define FRAME_NUM 5
// Static member initialization
bool WriteStream::dev_chn_enable[2] = {false, false};
bool ArduinoAudio::_vb_init = false;

static k_u32 audio_data_vb_create_pool(int blk_cnt,int block_size)
{
    k_u32 private_pool_id;
    k_vb_pool_config pool_config;
    memset(&pool_config, 0, sizeof(pool_config));
    pool_config.blk_cnt = blk_cnt;
    pool_config.blk_size = block_size;
    pool_config.mode = VB_REMAP_MODE_NOCACHE;
    private_pool_id = kd_mpi_vb_create_pool(&pool_config);
    printf("%s poolid %d\n", __func__,private_pool_id);

    return private_pool_id;
}

// AudioStream base class implementation
AudioStream::AudioStream(int rate, int channels, AudioFormat format, bool input, bool output,
               int input_device_index, int output_device_index, bool enable_codec,
               int frames_per_buffer, bool start)
    : _is_input(input), _is_output(output), _is_running(start),
      _rate(rate), _channels(channels), _format(format),
      _frames_per_buffer(frames_per_buffer),
      _input_device_index(input_device_index),
      _output_device_index(output_device_index),
      _enable_codec(enable_codec), _device_type(DEVICE_I2S) {

    if ((!input && !output) || (input && output)) {
        throw std::invalid_argument("Must specify an input or output stream.");
    }
}

// WriteStream implementation
WriteStream::WriteStream(int rate, int channels, AudioFormat format, bool input, bool output,
                         int input_device_index, int output_device_index, bool enable_codec,
                         int frames_per_buffer, bool start)
    : AudioStream(rate, channels, format, input, output, input_device_index,
             output_device_index, enable_codec, frames_per_buffer, start),
      _audio_frame(nullptr), _audio_handle(VB_INVALID_HANDLE), _start_stream(false) {

    if (output_device_index == -1 || output_device_index == 0) {
        _ao_dev = 0;
        _ao_chn = 0;
    } else if (output_device_index == 1) {
        _ao_dev = 0;
        _ao_chn = 1;
    } else {
        _ao_dev = 0;
        _ao_chn = 0;
    }
}

WriteStream::~WriteStream() {
    stop_stream();
    if (_audio_frame) {
        delete _audio_frame;
    }
}

void WriteStream::_init_audio_frame() {
    if (_audio_handle == -1) {

        int frame_size = _frames_per_buffer * _channels * 2;
        _pool_id = audio_data_vb_create_pool(1,frame_size);
        _audio_handle = kd_mpi_vb_get_block(_pool_id, frame_size, NULL);
        if (_audio_handle == -1) {
            throw std::runtime_error("kd_mpi_vb_get_block failed");
        }

        _audio_frame = new k_audio_frame();
        _audio_frame->len = frame_size;
        _audio_frame->pool_id = kd_mpi_vb_handle_to_pool_id(_audio_handle);
        _audio_frame->phys_addr = kd_mpi_vb_handle_to_phyaddr(_audio_handle);
        _audio_frame->virt_addr = kd_mpi_sys_mmap(_audio_frame->phys_addr, frame_size);
    }
}

void WriteStream::_deinit_audio_frame() {
    if (_audio_handle != -1) {
        kd_mpi_vb_release_block(_audio_handle);
        _audio_handle = -1;

        kd_mpi_vb_destory_pool(_pool_id);
        _pool_id = -1;
    }
    if (_audio_frame) {
        delete _audio_frame;
        _audio_frame = nullptr;
    }
}

static int  _ao_get_vol(Channel channel){
    int fd = -1;
    float vol_value;
    const float volume_zero_value = 7.0f - 50.0f;
    int ret = -1; // Default to failure
    int left_vol,right_vol;

    if (channel == LEFT_RIGHT) {
        printf("WriteStream::_ao_get_vol does not support LEFT_RIGHT\n");
        return -1;
    }

    // Open audio device
    fd = open("/dev/acodec_device", O_RDWR);
    if (fd < 0) {
        fprintf(stderr, "Could not open acodec_device: %s\n", strerror(errno));
        // Close device
        if (fd >= 0) {
            close(fd);
        }
        return -1;
    }

    if (channel == LEFT){
        // Get left channel volume (ioctl command 32)
        if (ioctl(fd, 32, &vol_value) != 0) {
            fprintf(stderr, "Failed to get left channel volume: %s\n", strerror(errno));
            // Close device
            if (fd >= 0) {
                close(fd);
            }
            return -1;
        }
        // Calculate left channel volume value (0-100 range mapping)
        if (vol_value < volume_zero_value) {
            left_vol = 0;
        } else if (vol_value > 7.0f) {
            left_vol = 100;
        } else {
            left_vol = (int)((vol_value - volume_zero_value) / 0.5f);
        }
    }
    else if (channel == RIGHT){
        if (ioctl(fd, 33, &vol_value) != 0) {
            fprintf(stderr, "Failed to get right channel volume: %s\n", strerror(errno));
            // Close device
            if (fd >= 0) {
                close(fd);
            }
            return -1;
        }
        // Calculate right channel volume value (0-100 range mapping)
        if (vol_value < volume_zero_value) {
            right_vol = 0;
        } else if (vol_value > 7.0f) {
            right_vol = 100;
        } else {
            right_vol = (int)((vol_value - volume_zero_value) / 0.5f);
        }
    }

    if (channel == LEFT){
        ret = left_vol;
    }
    else if (channel == RIGHT){
        ret = right_vol;
    }
    // Close device
    if (fd >= 0) {
        close(fd);
    }
    return ret;
}

static int  _ao_set_vol(int vol, Channel channel){

    int fd = -1;
    bool success = true;
    const float volume_base = 7.0f - 50.0f; // Base volume reference value
    float vol_value = volume_base + (float)vol * 0.5f;

    // Volume range limit
    if (vol_value > 6.5f) {
        vol_value = 7.0f;
    }
    if (vol_value < -120.0f) {
        vol_value = -120.0f;
    }

    // Open audio codec device
    fd = open("/dev/acodec_device", O_RDWR);
    if (fd < 0) {
        fprintf(stderr, "Could not open acodec_device: %s\n", strerror(errno));
        return -1;
    }

    // Set left channel volume (ioctl command 18)
    if (channel == LEFT_RIGHT || channel == LEFT) {
        if (ioctl(fd, 18, &vol_value) != 0) {
            success = false;
            fprintf(stderr, "Failed to set headphone left volume: %s\n", strerror(errno));
        }
    }

    // Set right channel volume (ioctl command 19)
    if ( channel == LEFT_RIGHT || channel == RIGHT) {
        if (ioctl(fd, 19, &vol_value) != 0) {
            success = false;
            fprintf(stderr, "Failed to set headphone right volume: %s\n", strerror(errno));
        }
    }

    // Close device file descriptor
    close(fd);

    return success ? 0 : -1;
}

static int _ao_swap_left_right(){
    int fd = -1;
    void* mem = NULL;
    const off_t REG_BASE = 0x9140E000; // Register base address
    const off_t REG_OFFSET = 0x10;     // Target register offset
    const uint32_t SWAP_BIT = 0x04;    // Left-right channel swap control bit

    // Open memory device
    fd = open("/dev/mem", O_RDWR | O_SYNC);
    if (fd < 0) {
        std::cerr << "Failed to open /dev/mem: " << strerror(errno) << std::endl;
        return -1;
    }

     // Map register address space
    mem = mmap(NULL, 4096, PROT_READ | PROT_WRITE, MAP_SHARED, fd, REG_BASE);
    if (mem == MAP_FAILED) {
        std::cerr << "Failed to mmap register: " << strerror(errno) << std::endl;
        close(fd);
        return -1;
    }

    // Manipulate register: set or clear swap bit
    uint32_t* reg = reinterpret_cast<uint32_t*>(reinterpret_cast<uint8_t*>(mem) + REG_OFFSET);
    uint32_t current = *reg;
    current |= SWAP_BIT; // Set bit when swap is needed

    *reg = current;

    // Clean up resources
    munmap(mem, 4096);
    close(fd);

    // Wait for hardware to take effect
    usleep(1000);

    return 0;
}

void WriteStream::start_stream() {
    if (!_start_stream) {
        _init_audio_frame();

        // Initialize device (once only)
        if (!(WriteStream::dev_chn_enable[0] && WriteStream::dev_chn_enable[1])) {
            k_aio_dev_attr aio_dev_attr;
            aio_dev_attr.audio_type = KD_AUDIO_OUTPUT_TYPE_I2S;
            aio_dev_attr.kd_audio_attr.i2s_attr.sample_rate = _rate;
            aio_dev_attr.kd_audio_attr.i2s_attr.bit_width = (k_audio_bit_width)_format;
            aio_dev_attr.kd_audio_attr.i2s_attr.chn_cnt = 2;
            aio_dev_attr.kd_audio_attr.i2s_attr.snd_mode = (_channels == 1) ? KD_AUDIO_SOUND_MODE_MONO : KD_AUDIO_SOUND_MODE_STEREO;
            aio_dev_attr.kd_audio_attr.i2s_attr.i2s_mode = K_STANDARD_MODE;
            aio_dev_attr.kd_audio_attr.i2s_attr.frame_num = FRAME_NUM;
            aio_dev_attr.kd_audio_attr.i2s_attr.point_num_per_frame = _frames_per_buffer;

            if (_enable_codec) {
                aio_dev_attr.kd_audio_attr.i2s_attr.i2s_type = K_AIO_I2STYPE_INNERCODEC;
            } else {
                aio_dev_attr.kd_audio_attr.i2s_attr.i2s_type = K_AIO_I2STYPE_EXTERN;
            }

            int ret = kd_mpi_ao_set_pub_attr(_ao_dev, &aio_dev_attr);
            if (ret != 0) {
                throw std::runtime_error("kd_mpi_ao_set_pub_attr failed: " + std::to_string(ret));
            }

            ret = kd_mpi_ao_enable(_ao_dev);
            if (ret != 0) {
                throw std::runtime_error("kd_mpi_ao_enable failed: " + std::to_string(ret));
            }

            WriteStream::dev_chn_enable[_ao_chn] = true;
        }

        int ret = kd_mpi_ao_enable_chn(_ao_dev, _ao_chn);
        if (ret != 0) {
            throw std::runtime_error("kd_mpi_ao_enable_chn failed: " + std::to_string(ret));
        }

        // Detect board type and swap left-right channels
        //this->swap_left_right();

        _start_stream = true;
    }
}

void WriteStream::stop_stream() {
    if (_start_stream) {
        int ret = kd_mpi_ao_disable_chn(_ao_dev, _ao_chn);
        if (ret != 0) {
            throw std::runtime_error("kd_mpi_ao_disable_chn failed: " + std::to_string(ret));
        }

        // Disable device (only when all channels are disabled)
        if (WriteStream::dev_chn_enable[0] ^ WriteStream::dev_chn_enable[1]) {
            ret = kd_mpi_ao_disable(_ao_dev);
            if (ret != 0) {
                throw std::runtime_error("kd_mpi_ao_disable failed: " + std::to_string(ret));
            }
        }

        _deinit_audio_frame();
        WriteStream::dev_chn_enable[_ao_chn] = false;
        _start_stream = false;
    }
}

int WriteStream::write(const std::vector<uint8_t>& data) {
    if (_start_stream && _audio_frame) {
        std::memcpy(_audio_frame->virt_addr, data.data(), data.size());
        return kd_mpi_ao_send_frame(_ao_dev, _ao_chn, _audio_frame, 1000);
    }
    return -1;
}

int WriteStream::read(std::vector<uint8_t>& data, int chn, bool block) {
    // Output stream does not support reading
    return -1;
}

void WriteStream::close() {
    _is_running = false;
}

int WriteStream::volume(int vol, Channel channel) {
    if (vol == -1) {
        return _ao_get_vol(channel);
    } else {
        return _ao_set_vol(vol, channel);
    }
}

int WriteStream::swap_left_right() {
    return _ao_swap_left_right();
}

// ReadStream implementation
ReadStream::ReadStream(int rate, int channels, AudioFormat format, bool input, bool output,
                       int input_device_index, int output_device_index, bool enable_codec,
                       int frames_per_buffer, bool start)
    : AudioStream(rate, channels, format, input, output, input_device_index,
             output_device_index, enable_codec, frames_per_buffer, start),
      _audio_frame(nullptr), _start_stream(false) {

    if (input_device_index == -1 || input_device_index == 0) {
        _ai_dev = 0;
        _ai_chn = 0;
        _device_type = DEVICE_I2S;
    } else if (input_device_index == 1) {
        _ai_dev = 1;
        _ai_chn = 0;
        _pdm_chncnt = _channels / 2;
        _device_type = DEVICE_PDM;
    } else {
        _ai_dev = 0;
        _ai_chn = 0;
        _device_type = DEVICE_I2S;
    }
}

ReadStream::~ReadStream() {
    stop_stream();
    if (_audio_frame) {
        delete _audio_frame;
    }
}

static int _ai_get_vol(Channel channel)
{
    int fd = -1;
    float vol_value;
    int vol;
    const float volume_zero_value = 30.0f - 50.0f; // Calculate zero volume reference

    if (channel == LEFT_RIGHT) {
        printf("ReadStream::_ai_get_vol does not support LEFT_RIGHT\n");
        return -1;
    }

      // Open audio device
    fd = open("/dev/acodec_device", O_RDWR);
    if (fd < 0) {
        printf("Could not open acodec_device\n");
        return -1;
    }

    if (channel == LEFT){
        // Get left channel volume
        if (ioctl(fd, 26, &vol_value) != 0) {
            printf("Failed to get microphone left volume\n");
            close(fd);
            return -1;
        }
        // Calculate left channel volume percentage (0-100)
        if (vol_value < volume_zero_value) {
            vol = 0;
        } else if (vol_value > 30.0f) {
            vol = 100;
        } else {
            vol = (int)((vol_value - volume_zero_value) / 0.5f);
        }
    }
    else if (channel == RIGHT){
        // Get right channel volume
        if (ioctl(fd, 27, &vol_value) != 0) {
            printf("Failed to get microphone right volume\n");
            close(fd);
            return -1;
        }
        // Calculate right channel volume percentage (0-100)
        if (vol_value < volume_zero_value) {
            vol = 0;
        } else if (vol_value > 30.0f) {
            vol = 100;
        } else {
            vol = (int)((vol_value - volume_zero_value) / 0.5f);
        }
    }

    // Close device
    close(fd);

    return vol;
}

static int  _ai_set_vol(int vol, Channel channel)
{
    int fd = -1;
    bool success = true;
    const float volume_zero_value = 30.0f - 50.0f; // Base reference value
    float vol_value = volume_zero_value + static_cast<float>(vol) * 0.5f;

    // Volume range limit
    if (vol_value > 30.0f) {
        vol_value = 30.0f;
    }
    if (vol_value < -97.0f) {
        vol_value = -97.0f;
    }

    // Open audio device
    fd = open("/dev/acodec_device", O_RDWR);
    if (fd < 0) {
        std::cerr << "Could not open acodec_device" << std::endl;
        return false;
    }

    if (channel == LEFT || channel == LEFT_RIGHT) {
        // Set left channel volume
        if (ioctl(fd, 12, &vol_value) != 0) {
            success = false;
            std::cerr << "Failed to set microphone left volume" << std::endl;
        }
    }

    if (channel == RIGHT || channel == LEFT_RIGHT) {
        // Set right channel volume
        if (ioctl(fd, 13, &vol_value) != 0) {
            success = false;
            std::cerr << "Failed to set microphone right volume" << std::endl;
        }
    }

    // Close device
    close(fd);
    return 0;
}

static int _ai_swap_left_right()
{
    int fd = -1;
    void* mem = NULL;
    int ret = -1; // Default to failure

    // Open memory device
    fd = open("/dev/mem", O_RDWR | O_SYNC);
    if (fd < 0) {
        fprintf(stderr, "Failed to open /dev/mem: %s\n", strerror(errno));
        // Release resources
        if (mem != NULL && mem != MAP_FAILED) {
            munmap(mem, 4096);
        }
        if (fd >= 0) {
            close(fd);
        }
        return ret;
    }

    // Map physical address to user space (4096-byte page size)
    mem = mmap(NULL, 4096, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0x9140E000);
    if (mem == MAP_FAILED) {
        fprintf(stderr, "Failed to mmap: %s\n", strerror(errno));
        // Release resources
        if (mem != NULL && mem != MAP_FAILED) {
            munmap(mem, 4096);
        }
        if (fd >= 0) {
            close(fd);
        }
        return ret;
    }

    // Manipulate register: clear lower 2 bits then set based on enable
    uint32_t* reg_addr = (uint32_t*)((uint8_t*)mem + 0x08);
    uint32_t reg = *reg_addr;
    reg |= 0x03; // Set lower 2 bits to 1 when enabling

    *reg_addr = reg;

    // Wait for hardware to take effect
    usleep(1000);

    ret = 0; // Success

    // Release resources
    if (mem != NULL && mem != MAP_FAILED) {
        munmap(mem, 4096);
    }
    if (fd >= 0) {
        close(fd);
    }
    return ret;
}

void ReadStream::start_stream() {
    if (!_start_stream) {
        _audio_frame = new k_audio_frame();

        if (_device_type == DEVICE_I2S) {
            k_aio_dev_attr aio_dev_attr = {};
            aio_dev_attr.audio_type = KD_AUDIO_INPUT_TYPE_I2S;
            aio_dev_attr.kd_audio_attr.i2s_attr.sample_rate = _rate;
            aio_dev_attr.kd_audio_attr.i2s_attr.bit_width = (k_audio_bit_width)_format;
            aio_dev_attr.kd_audio_attr.i2s_attr.chn_cnt = 2;
            aio_dev_attr.kd_audio_attr.i2s_attr.snd_mode = (_channels == 1) ? KD_AUDIO_SOUND_MODE_MONO : KD_AUDIO_SOUND_MODE_STEREO;
            aio_dev_attr.kd_audio_attr.i2s_attr.i2s_mode = K_STANDARD_MODE;
            aio_dev_attr.kd_audio_attr.i2s_attr.frame_num = FRAME_NUM;
            aio_dev_attr.kd_audio_attr.i2s_attr.point_num_per_frame = _frames_per_buffer;

            if (_enable_codec) {
                aio_dev_attr.kd_audio_attr.i2s_attr.i2s_type = K_AIO_I2STYPE_INNERCODEC;
            } else {
                aio_dev_attr.kd_audio_attr.i2s_attr.i2s_type = K_AIO_I2STYPE_EXTERN;
            }

            int ret = kd_mpi_ai_set_pub_attr(_ai_dev, &aio_dev_attr);
            if (ret != 0) {
                throw std::runtime_error("kd_mpi_ai_set_pub_attr failed: " + std::to_string(ret));
            }

            ret = kd_mpi_ai_enable(_ai_dev);
            if (ret != 0) {
                throw std::runtime_error("kd_mpi_ai_enable failed: " + std::to_string(ret));
            }

            ret = kd_mpi_ai_enable_chn(_ai_dev, _ai_chn);
            if (ret != 0) {
                throw std::runtime_error("kd_mpi_ai_enable_chn failed: " + std::to_string(ret));
            }
        } else if (_device_type == DEVICE_PDM) {
            k_aio_dev_attr aio_dev_attr = {};
            aio_dev_attr.audio_type = KD_AUDIO_INPUT_TYPE_PDM;
            aio_dev_attr.kd_audio_attr.pdm_attr.sample_rate = _rate;
            aio_dev_attr.kd_audio_attr.pdm_attr.bit_width = (k_audio_bit_width)_format;
            aio_dev_attr.kd_audio_attr.pdm_attr.chn_cnt = _pdm_chncnt;
            aio_dev_attr.kd_audio_attr.pdm_attr.snd_mode = KD_AUDIO_SOUND_MODE_STEREO;
            aio_dev_attr.kd_audio_attr.pdm_attr.frame_num = FRAME_NUM;
            aio_dev_attr.kd_audio_attr.pdm_attr.pdm_oversample = KD_AUDIO_PDM_INPUT_OVERSAMPLE_64;
            aio_dev_attr.kd_audio_attr.pdm_attr.point_num_per_frame = _frames_per_buffer;

            int ret = kd_mpi_ai_set_pub_attr(_ai_dev, &aio_dev_attr);
            if (ret != 0) {
                throw std::runtime_error("kd_mpi_ai_set_pub_attr failed: " + std::to_string(ret));
            }

            ret = kd_mpi_ai_enable(_ai_dev);
            if (ret != 0) {
                throw std::runtime_error("kd_mpi_ai_enable failed: " + std::to_string(ret));
            }

            for (int i = 0; i < _pdm_chncnt; i++) {
                ret = kd_mpi_ai_enable_chn(_ai_dev, i);
                if (ret != 0) {
                    throw std::runtime_error("kd_mpi_ai_enable_chn failed: " + std::to_string(ret));
                }
            }
        }

        _start_stream = true;
    }
}

void ReadStream::stop_stream() {
    if (_start_stream) {
        if (_device_type == DEVICE_I2S) {
            int ret = kd_mpi_ai_disable_chn(_ai_dev, _ai_chn);
            if (ret != 0) {
                throw std::runtime_error("kd_mpi_ai_disable_chn failed: " + std::to_string(ret));
            }
        } else if (_device_type == DEVICE_PDM) {
            for (int i = _pdm_chncnt - 1; i >= 0; i--) {
                int ret = kd_mpi_ai_disable_chn(_ai_dev, i);
                if (ret != 0) {
                    throw std::runtime_error("kd_mpi_ai_disable_chn failed: " + std::to_string(ret));
                }
            }
        }

        int ret = kd_mpi_ai_disable(_ai_dev);
        if (ret != 0) {
            throw std::runtime_error("kd_mpi_ai_disable failed: " + std::to_string(ret));
        }

        _start_stream = false;
    }
}

int ReadStream::read(std::vector<uint8_t>& data, int chn, bool block) {
    if (!_start_stream) {
        return -1;
    }

    if (_device_type == DEVICE_I2S) {
        int ret = kd_mpi_ai_get_frame(_ai_dev, _ai_chn, _audio_frame, block ? -1 : 1000);
        if (ret == 0) {
            void* vir_data = kd_mpi_sys_mmap(_audio_frame->phys_addr, _audio_frame->len);
            data.resize(_audio_frame->len);
            std::memcpy(data.data(), vir_data, _audio_frame->len);
            kd_mpi_sys_munmap(vir_data, _audio_frame->len);
            kd_mpi_ai_release_frame(_ai_dev, _ai_chn, _audio_frame);
            return 0;
        }
    } else if (_device_type == DEVICE_PDM) {
        if (chn < 0 || chn >= _pdm_chncnt) {
            throw std::invalid_argument("pdm channel " + std::to_string(chn) + " error");
        }

        int ret = kd_mpi_ai_get_frame(_ai_dev, chn, _audio_frame, block ? -1 : 1000);
        if (ret == 0) {
            void* vir_data = kd_mpi_sys_mmap(_audio_frame->phys_addr, _audio_frame->len);
            data.resize(_audio_frame->len);
            std::memcpy(data.data(), vir_data, _audio_frame->len);
            kd_mpi_sys_munmap(vir_data, _audio_frame->len);
            kd_mpi_ai_release_frame(_ai_dev, chn, _audio_frame);
            return 0;
        }
    }

    return -1;
}

int ReadStream::write(const std::vector<uint8_t>& data) {
    // Input stream does not support writing
    return -1;
}

void ReadStream::close() {
    _is_running = false;
}

int ReadStream::volume(int vol, Channel channel) {
    if (vol == -1) {
        return _ai_get_vol(channel);
    } else {
        return _ai_set_vol(vol, channel);
    }
}

int ReadStream::swap_left_right() {
    return _ai_swap_left_right();
}

int ReadStream::enable_audio3a(int audio3a_value) {
    k_ai_vqe_enable aio_vqe_enable = {};

    if (audio3a_value & AUDIO_3A_ENABLE_ANS) {
        aio_vqe_enable.ans_enable = K_TRUE;
    }
    if (audio3a_value & AUDIO_3A_ENABLE_AGC) {
        aio_vqe_enable.agc_enable = K_TRUE;
    }
    if (audio3a_value & AUDIO_3A_ENABLE_AEC) {
        aio_vqe_enable.aec_enable = K_TRUE;
    }

    int ret = kd_mpi_ai_set_vqe_attr(_ai_dev, _ai_chn, aio_vqe_enable);
    if (ret != 0) {
        throw std::runtime_error("kd_mpi_ai_set_vqe_attr failed: " + std::to_string(ret));
    }

    return ret;
}

int ReadStream::audio3a_send_far_echo_frame(const std::vector<uint8_t>& frame_data, int data_len) {
    k_vb_blk_handle handle = kd_mpi_vb_get_block(-1, data_len, NULL);
    k_u64  phys_addr = kd_mpi_vb_handle_to_phyaddr(handle);
    void* vir_data = kd_mpi_sys_mmap(phys_addr, data_len);

    k_audio_frame frame;
    frame.len = data_len;
    std::memcpy(vir_data, frame_data.data(), data_len);
    frame.phys_addr = phys_addr;
    kd_mpi_sys_munmap(vir_data, frame.len);

    int ret = kd_mpi_ai_send_far_echo_frame(_ai_dev, _ai_chn, &frame, 100);
    kd_mpi_vb_release_block(handle);

    return ret;
}

// ArduinoAudio implementation
ArduinoAudio::ArduinoAudio() = default;

ArduinoAudio::~ArduinoAudio() {
}

AudioStream* ArduinoAudio::open(int rate, int channels, AudioFormat format,
                                     bool input, bool output,
                                     int input_device_index, int output_device_index,
                                     bool enable_codec, int frames_per_buffer,
                                     bool start) {
    AudioStream* stream;

    if (output) {
        stream = new WriteStream(rate, channels, format, input, output,
                                              input_device_index, output_device_index,
                                              enable_codec, frames_per_buffer, start);
    } else {
        stream = new ReadStream(rate, channels, format, input, output,
                                             input_device_index, output_device_index,
                                             enable_codec, frames_per_buffer, start);
    }

    _streams.push_back(stream);
    return stream;
}

void ArduinoAudio::close(AudioStream* stream) {
    std::vector<AudioStream*>::iterator itr = _streams.begin();

    for (; itr != _streams.end(); ++itr) {
        if (*itr == stream) {
            break;
        }
    }

    if (itr == _streams.end()) {
        throw std::invalid_argument("AudioStream not found");
    }

    _streams.erase(itr);
    stream->close();
    delete stream;
}

void ArduinoAudio::_remove_stream(AudioStream* stream) {
    std::vector<AudioStream*>::iterator itr = _streams.begin();

    for (; itr != _streams.end(); ++itr) {
        if (*itr == stream) {
            break;
        }
    }

    if (itr != _streams.end()) {
        _streams.erase(itr);
    }
}

int ArduinoAudio::get_sample_size(int format) {
    switch (format) {
        case paInt16: return 2;
        case paInt24: return 3;
        case paInt32: return 4;
        default: return -1;
    }
}

int ArduinoAudio::get_format_from_width(int width) {
    switch (width) {
        case 2: return paInt16;
        case 3: return paInt24;
        case 4: return paInt32;
        default: return -1;
    }
}

int ArduinoAudio::pdm_audio_pin_config(int clk_pin, int data0_pin, int data1_pin, int data2_pin, int data3_pin)
{
    // Pin configuration constant definitions
    const int PDM_CLK_PIN_ALLOWED       = 26;   // Allowed clock pin
    const int PDM_DATA0_PIN_OPTION1     = 27;   // Data0 pin option 1
    const int PDM_DATA0_PIN_OPTION2     = 37;   // Data0 pin option 2
    const int PDM_DATA1_PIN_OPTION1     = 28;   // Data1 pin option 1
    const int PDM_DATA1_PIN_OPTION2     = 35;   // Data1 pin option 2
    const int PDM_DATA2_PIN_ALLOWED     = 36;   // Allowed data2 pin
    const int PDM_DATA3_PIN_ALLOWED     = 34;   // Allowed data3 pin

    // Define pin configuration mapping structure (contains pin, corresponding function and configuration value)
    struct PinConfig {
        int pin;
        fpioa_func_t func;       // PDM function corresponding to this pin
        uint32_t config_value;   // Pin configuration value
    };

    // Pin-function-configuration value mapping table (classified by pin function)
    const PinConfig pin_configs[] = {
        {PDM_CLK_PIN_ALLOWED,  PDM_CLK, 0x188E},
        {PDM_DATA0_PIN_OPTION1,PDM_IN0, 0x8000190F},
        {PDM_DATA0_PIN_OPTION2,IIS_D_OUT1_PDM_IN0, 0x8000110E},
        {PDM_DATA1_PIN_OPTION1,PDM_IN1, 0x8000190F},
        {PDM_DATA1_PIN_OPTION2, IIS_D_OUT0_PDM_IN1,0x8000110E},
        {PDM_DATA2_PIN_ALLOWED,PDM_IN2, 0x8000110E},
        {PDM_DATA3_PIN_ALLOWED,PDM_IN3, 0x8000110E}
    };

    // Verify clock pin (must match allowed pin)
    if (clk_pin != PDM_CLK_PIN_ALLOWED) {
        printf("Error: PDM clock pin must be %d\n", PDM_CLK_PIN_ALLOWED);
        return -1;
    }

    // Verify data0 pin (must be one of the allowed options, 0 means disabled)
    if (data0_pin > 0 && data0_pin != PDM_DATA0_PIN_OPTION1 && data0_pin != PDM_DATA0_PIN_OPTION2) {
        printf("Error: PDM data0 pin must be %d or %d\n", PDM_DATA0_PIN_OPTION1, PDM_DATA0_PIN_OPTION2);
        return -1;
    }

    // Verify data1 pin (if enabled, must be one of the allowed options)
    if (data1_pin > 0 && data1_pin != PDM_DATA1_PIN_OPTION1 && data1_pin != PDM_DATA1_PIN_OPTION2) {
        printf("Error: PDM data1 pin must be %d or %d\n", PDM_DATA1_PIN_OPTION1, PDM_DATA1_PIN_OPTION2);
        return -1;
    }

    // Verify data2 pin (if enabled, must match allowed pin)
    if (data2_pin > 0 && data2_pin != PDM_DATA2_PIN_ALLOWED) {
        printf("Error: PDM data2 pin must be %d\n", PDM_DATA2_PIN_ALLOWED);
        return -1;
    }

    // Verify data3 pin (if enabled, must match allowed pin)
    if (data3_pin > 0 && data3_pin != PDM_DATA3_PIN_ALLOWED) {
        printf("Error: PDM data3 pin must be %d\n", PDM_DATA3_PIN_ALLOWED);
        return -1;
    }

    // Function to configure a single pin: find corresponding function and configuration value by pin and apply
    auto configure_pin = [&pin_configs](int pin) -> int {
        // Find configuration corresponding to the pin
        for (const auto& config : pin_configs) {
            if (config.pin == pin) {
                // Set pin function and configuration
                drv_fpioa_set_pin_cfg(pin, config.config_value);
                return 0; // Configuration successful
            }
        }
        printf("Error: No configuration information found for pin %d\n", pin);
        return -1;
    };

    // Configure all enabled pins (configure in order, 0 means skip)
    const int pins_to_configure[] = {clk_pin, data0_pin, data1_pin, data2_pin, data3_pin};
    for (int pin : pins_to_configure) {
        if (pin > 0) { // Only configure enabled pins (>0 means enabled)
            if (configure_pin(pin) != 0) {
                return -1; // Return immediately if configuration fails
            }
        }
    }

    return 0; // All pins configured successfully
}