/**
 * Copyright (c) 2009 - 2017, Nordic Semiconductor ASA
 * 
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 * 
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 
 * 2. Redistributions in binary form, except as embedded into a Nordic
 *    Semiconductor ASA integrated circuit in a product or a software update for
 *    such product, must reproduce the above copyright notice, this list of
 *    conditions and the following disclaimer in the documentation and/or other
 *    materials provided with the distribution.
 * 
 * 3. Neither the name of Nordic Semiconductor ASA nor the names of its
 *    contributors may be used to endorse or promote products derived from this
 *    software without specific prior written permission.
 * 
 * 4. This software, with or without modification, must only be used with a
 *    Nordic Semiconductor ASA integrated circuit.
 * 
 * 5. Any software provided in binary form under this license must not be reverse
 *    engineered, decompiled, modified and/or disassembled.
 * 
 * THIS SOFTWARE IS PROVIDED BY NORDIC SEMICONDUCTOR ASA "AS IS" AND ANY EXPRESS
 * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL NORDIC SEMICONDUCTOR ASA OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * 
 */
/** @file
* @brief Example template project.
* @defgroup nrf_templates_example Example Template
*
*/

#include <stdbool.h>
#include <stdint.h>

#include "nrf.h"
#include "nordic_common.h"

#include "nrf_gpio.h"
#include "nrf_spim.h"
#include "nrf_rng.h"

#include "nrf_atomic.h"
#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_backend_rtt.h"
#include "nrf_delay.h"

#include "nrf_cli.h"
#include "nrf_cli_rtt.h"
#include "nrf_cli_uart.h"

#include "app_util.h"
#include "app_timer.h"
#include "nrf_drv_clock.h"
#include "nrf_stack_guard.h"
#include "stack_profiler.h"
#include "m_audio_gauges.h"

#if CONFIG_AUDIO_CODEC != 4
#error Only SBC codec is supported
#endif

// Using Smart Remote 3 configuration files for easy reuse of Opus integration
#ifndef CONFIG_BOARD_NRF52832_CUSTOM
#define define CONFIG_BOARD_NRF52832_CUSTOM
#endif /* #ifndef CONFIG_BOARD_NRF52832_CUSTOM */
#include "sr3_config.h"
// #include "sr3_config_nrf52832_custom.h"

#include "sbc_encoder.h"
#include "oi_codec_sbc.h"

#define SBC_MAX_FRAME_SIZE 4096

#define USE_WHITE_NOISE_SAMPLE 0

#if   (CONFIG_AUDIO_SAMPLING_FREQUENCY == 16000)
# define SBC_SAMPLING_FREQUENCY SBC_sf16000
#elif (CONFIG_AUDIO_SAMPLING_FREQUENCY == 32000)
# define SBC_SAMPLING_FREQUENCY SBC_sf32000
#elif (CONFIG_AUDIO_SAMPLING_FREQUENCY == 48000)
# define SBC_SAMPLING_FREQUENCY SBC_sf48000
#else
# error "Selected sampling frequency is not supported by the SBC codec!"
#endif

#if   (CONFIG_SBC_ALLOCATION == 0)
# define SBC_ALLOCATION_METHOD  SBC_LOUDNESS
#elif (CONFIG_SBC_ALLOCATION == 1)
# define SBC_ALLOCATION_METHOD  SBC_SNR
#else
# error "Unsupported allocation method selected for SBC codec"
#endif

#if CONFIG_CL_RTT_ENABLED
#error Test
#if CONFIG_CL_RTT_INTERFACE == CONFIG_CL_INTERFACE_CLI
/**@brief Logger over RTT instance. */
// NRF_LOG_BACKEND_RTT_DEF(m_rtt_log_backend);

#elif CONFIG_CL_RTT_INTERFACE == CONFIG_CL_INTERFACE_CLI
#error Test
/**@brief Console over RTT instance. */
NRF_CLI_RTT_DEF(m_cli_rtt);
NRF_CLI_DEF(s_rtt_cli, "SR3-RTT> ", &m_cli_rtt.transport, '\n', CONFIG_LOG_RTT_QSIZE);
#else
#error "Unsupported RTT mode, please configure RTT as CLI or Logger backend"
#endif
#endif

#if !CONFIG_CLI_ENABLED
#error CLI must be enabled
#endif

static SBC_ENC_PARAMS               m_sbc_enc_params;
static OI_CODEC_SBC_DECODER_CONTEXT m_sbc_dec_params;
// static OI_UINT32                    m_sbc_dec_params_buffer[(2 /* channels */ * sizeof(uint32_t) * SBC_MAX_BANDS * SBC_MAX_BLOCKS) / sizeof(uint32_t)];
static OI_UINT32                    m_sbc_dec_params_buffer[1024];

// static uint8_t                      m_opus_complexity = CONFIG_OPUS_COMPLEXITY;
// static int32_t                      m_opus_bitrate    = ((CONFIG_OPUS_BITRATE != 0) ? CONFIG_OPUS_BITRATE : OPUS_AUTO);
// static bool                         m_opus_vbr        = ((CONFIG_OPUS_BITRATE == 0) || (CONFIG_OPUS_VBR_ENABLED != 0));
static int16_t                      m_channel_count          = 2;
static uint32_t                     m_sbc_frame_size_us      = (CONFIG_AUDIO_FRAME_SIZE_SAMPLES * 1000000 / CONFIG_AUDIO_SAMPLING_FREQUENCY);
static uint32_t                     m_sbc_frame_size_samples = CONFIG_AUDIO_FRAME_SIZE_SAMPLES;
static uint32_t                     m_sbc_frame_size_bytes   = CONFIG_AUDIO_FRAME_SIZE_BYTES;
static int16_t                      m_sbc_subbands           = CONFIG_SBC_SUBBANDS;
static int16_t                      m_sbc_blocks             = CONFIG_SBC_BLOCKS;
static int16_t                      m_sbc_bitpool            = CONFIG_SBC_BITPOOL;
static uint32_t                     m_sbc_sampling_rate      = CONFIG_AUDIO_SAMPLING_FREQUENCY;

static nrf_atomic_flag_t           m_run_test_flag = 0;

#define CLI_EXAMPLE_LOG_QUEUE_SIZE  (32)
#if CONFIG_CL_RTT_ENABLED
NRF_CLI_RTT_DEF(m_cli_rtt_transport);
NRF_CLI_DEF(m_cli_rtt,
            "rtt_cli:~$ ",
            &m_cli_rtt_transport.transport,
            '\n',
            CLI_EXAMPLE_LOG_QUEUE_SIZE);
#endif
#if CONFIG_CL_UART_ENABLED
NRF_CLI_UART_DEF(m_cli_uart_transport, 0, 512, 32);
NRF_CLI_DEF(m_cli_uart,
            "uart_cli:~$ ",
            &m_cli_uart_transport.transport,
            '\r',
            CLI_EXAMPLE_LOG_QUEUE_SIZE);
#endif


static m_audio_bitrate_gauge_t  m_bitrate_gauge;
static m_audio_cpu_gauge_t      m_encoding_cpu_gauge;
static m_audio_cpu_gauge_t      m_decoding_cpu_gauge;

extern const uint8_t sample_1_delivering_the_goods_48000Hz_16bit_stereo_2sec[384000];
extern const uint8_t sample_1_delivering_the_goods_48000Hz_16bit_mono_2sec[192000];

#define PCM_SAMPLE_MONO   sample_1_delivering_the_goods_48000Hz_16bit_mono_2sec
#define PCM_SAMPLE_STEREO sample_1_delivering_the_goods_48000Hz_16bit_stereo_2sec

static void drv_audio_codec_log_config(const char *action)
{
#if   (CONFIG_SBC_MODE == CONFIG_SBC_MODE_MSBC)
    NRF_LOG_INFO("SBC Codec selected (mSBC mode, frame: %u ms)",
                 CONFIG_AUDIO_FRAME_SIZE_MS);
#elif (CONFIG_SBC_MODE == CONFIG_SBC_MODE_CUSTOM)
    NRF_LOG_INFO("SBC Codec selected (blocks: %u, subbands: %u, bitpool: %u, frame: %u ms)",
                 m_sbc_enc_params.s16NumOfBlocks,
                 m_sbc_enc_params.s16NumOfSubBands,
                 m_sbc_enc_params.s16BitPool,
                 CONFIG_AUDIO_FRAME_SIZE_MS);
#else
# error "Invalid SBC Mode!"
#endif

    NRF_LOG_INFO("Sampling frequency: %d Hz", m_sbc_sampling_rate);
}

static void hftimer_start(void)
{
    NRF_TIMER1->TASKS_STOP = 1;
    NRF_TIMER1->PRESCALER = 0;
    NRF_TIMER1->BITMODE = TIMER_BITMODE_BITMODE_32Bit << TIMER_BITMODE_BITMODE_Pos;
    NRF_TIMER1->SHORTS = 0;
    NRF_TIMER1->CC[0] = 0;
    NRF_TIMER1->TASKS_CLEAR = 1;
    NRF_TIMER1->TASKS_START = 1;
}

static uint32_t hftimer_stop(void)
{
    NRF_TIMER1->TASKS_CAPTURE[0] = 1;
    NRF_TIMER1->TASKS_STOP = 1;

    return NRF_TIMER1->CC[0];
}

static void sbc_encoder_configure(void)
{
    NRF_LOG_INFO("Configuring encoder");

    m_sbc_enc_params.s16ChannelMode         = m_channel_count == 1 ? SBC_MONO : SBC_JOINT_STEREO;
    m_sbc_enc_params.s16NumOfChannels       = m_channel_count;
    m_sbc_enc_params.s16NumOfBlocks         = m_sbc_blocks;
    m_sbc_enc_params.s16NumOfSubBands       = m_sbc_subbands;
    m_sbc_enc_params.s16BitPool             = m_sbc_bitpool;
    m_sbc_enc_params.s16AllocationMethod    = SBC_ALLOCATION_METHOD;
    m_sbc_enc_params.mSBCEnabled            = (CONFIG_SBC_MODE == CONFIG_SBC_MODE_MSBC) ? 1 : 0;
    switch (m_sbc_sampling_rate)
    {
        case 16000:
            m_sbc_enc_params.s16SamplingFreq = SBC_sf16000;
            break;
        case 48000:
            m_sbc_enc_params.s16SamplingFreq = SBC_sf48000;
            break;
        default:
            APP_ERROR_CHECK_BOOL(false);
            break;
    }
    
    APP_ERROR_CHECK_BOOL(m_sbc_enc_params.mSBCEnabled == 0);

    SBC_Encoder_Init(&m_sbc_enc_params);
}

static void sbc_decoder_configure(void)
{
    OI_STATUS status = OI_OK;

    NRF_LOG_INFO("Configuring decoder");

    status = OI_CODEC_SBC_DecoderReset(&m_sbc_dec_params, m_sbc_dec_params_buffer, sizeof(m_sbc_dec_params_buffer), 2, 2, false);
    if (!OI_SUCCESS(status))
    {
        NRF_LOG_ERROR("Failed to initialize decoder: %d", status);
        APP_ERROR_CHECK_BOOL(false);
    }
}

static void sbc_param_update_and_init(void)
{
    uint32_t delay_us;
    uint32_t bitrate_kbps;

    m_sbc_frame_size_samples = m_sbc_blocks * m_sbc_subbands;
    m_sbc_frame_size_bytes   = (4 + ((4 * m_sbc_subbands) / 8) + CEIL_DIV(m_sbc_blocks * m_sbc_bitpool, 8));
    m_sbc_frame_size_us      = ((m_sbc_frame_size_samples * 1000000) / m_sbc_sampling_rate);

    sbc_encoder_configure();
    sbc_decoder_configure();

    drv_audio_codec_log_config("configured");

    delay_us = ((m_sbc_blocks + 10) * m_sbc_subbands - 2);
    delay_us *= (1000000 / m_sbc_sampling_rate);

    bitrate_kbps = ((m_sbc_sampling_rate / m_sbc_frame_size_samples) * m_sbc_frame_size_bytes * 8) / 1000;

    NRF_LOG_INFO("Algorithmic delay: %d us", delay_us);
    NRF_LOG_INFO("Bitrate:           %d kbps", bitrate_kbps);

    if (m_channel_count == 1)
    {
        NRF_LOG_INFO("Using sample: %s", (uint32_t) STRINGIFY(PCM_SAMPLE_MONO));
    }
    else
    {
        NRF_LOG_INFO("Using sample: %s", (uint32_t) STRINGIFY(PCM_SAMPLE_STEREO));
    }
}

uint16_t m_audio_gauge_get_avg_bitrate(const m_audio_bitrate_gauge_t *p_gauge)
{
    uint32_t frames;
    uint32_t bytes;

    CRITICAL_REGION_ENTER();
    frames  = p_gauge->frames;
    bytes   = p_gauge->bytes;
    CRITICAL_REGION_EXIT();

    if (frames == 0)
    {
        return 0;
    }

    return (8ull * bytes * m_sbc_sampling_rate) / (1000ull * frames * (m_sbc_blocks * m_sbc_subbands));
}

void m_audio_measure_bitrate(m_audio_bitrate_gauge_t *p_gauge, unsigned int bytes)
{
    ASSERT(p_gauge != NULL);

    p_gauge->cur_bitrate    = 8ul * bytes * m_sbc_sampling_rate / (m_sbc_blocks * m_sbc_subbands) / 1000;
    p_gauge->frames         += 1;
    p_gauge->bytes          += bytes;

    if (p_gauge->max_bitrate < p_gauge->cur_bitrate)
    {
        p_gauge->max_bitrate = p_gauge->cur_bitrate;
    }

    if (p_gauge->min_bitrate > p_gauge->cur_bitrate)
    {
        p_gauge->min_bitrate = p_gauge->cur_bitrate;
    }
}

static void run_loopback_test(void)
{
    static int16_t frame_buf_uncompressed[16 * 8 * 2];
    static uint8_t frame_buf_compressed[SBC_MAX_FRAME_SIZE];
    size_t         num_frames_in_sample;
    uint32_t       compressed_frame_size;
    uint32_t       uncompressed_frame_size;
    // int            decompressed_frame_size; 
    uint32_t       encoder_stack_usage;
    uint32_t       encoder_stack_usage_max;
    uint32_t       decoder_stack_usage;
    uint32_t       decoder_stack_usage_max;
    uint32_t       encoder_timer_sum;
    uint32_t       decoder_timer_sum;
    int16_t      * p_sample;
    OI_BYTE      * p_framedata;
    OI_STATUS      decode_status;

    m_audio_bitrate_gauge_reset(&m_bitrate_gauge);
    m_audio_cpu_gauge_reset(&m_encoding_cpu_gauge);
    m_audio_cpu_gauge_reset(&m_decoding_cpu_gauge);

    APP_ERROR_CHECK_BOOL(m_channel_count <= 2);

    if (m_channel_count == 1)
    {
        p_sample             = (int16_t*)PCM_SAMPLE_MONO;
        num_frames_in_sample = (sizeof(PCM_SAMPLE_MONO) / sizeof(int16_t)) / (m_sbc_frame_size_samples * m_channel_count);
        NRF_LOG_INFO("Running loopback of sample: %s (%d seconds)", (uint32_t) STRINGIFY(PCM_SAMPLE_MONO), (sizeof(PCM_SAMPLE_MONO) / sizeof(int16_t)) / m_sbc_sampling_rate);
    }
    else
    {
        p_sample             = (int16_t*)PCM_SAMPLE_STEREO;
        num_frames_in_sample = (sizeof(PCM_SAMPLE_STEREO) / sizeof(int16_t)) / (m_sbc_frame_size_samples * m_channel_count);
        NRF_LOG_INFO("Running loopback of sample: %s (%d seconds)", (uint32_t) STRINGIFY(PCM_SAMPLE_STEREO), (sizeof(PCM_SAMPLE_STEREO) / (sizeof(int16_t) * 2)) / m_sbc_sampling_rate);
    }

    encoder_stack_usage_max = 0;
    decoder_stack_usage_max = 0;

    encoder_timer_sum = 0;
    decoder_timer_sum = 0;


    NRF_LOG_DEBUG("m_sbc_frame_size_samples: %d", m_sbc_frame_size_samples);
    NRF_LOG_DEBUG("m_channel_count:          %d", m_channel_count);
    NRF_LOG_DEBUG("num_frames_in_sample:     %d", num_frames_in_sample);

    for (int i = 0; i < num_frames_in_sample; ++i)
    {
        uncompressed_frame_size = m_sbc_frame_size_samples * m_channel_count * sizeof(int16_t);
#if USE_WHITE_NOISE_SAMPLE
        (void) p_sample;
        for (int j = 0; j < (uncompressed_frame_size / sizeof(int16_t)); ++j)
        {
            frame_buf_uncompressed[j] = (int16_t) rand();
        }
#else
        // Copy frame from sample
        memcpy(frame_buf_uncompressed, &((int16_t*)p_sample)[i * m_sbc_frame_size_samples * m_channel_count], uncompressed_frame_size);
#endif

#if CONFIG_STACK_PROFILER_ENABLED
    // Initialize stack profiler.
    stack_profiler_init();
#endif

        // Compress frame
        nrf_gpio_pin_set(NRF_GPIO_PIN_MAP(0, 2));
        nrf_gpio_pin_clear(NRF_GPIO_PIN_MAP(0, 2));
        nrf_gpio_pin_set(NRF_GPIO_PIN_MAP(0, 2));
        m_audio_measure_cpu_usage_start(&m_encoding_cpu_gauge);
        hftimer_start();

        m_sbc_enc_params.ps16PcmBuffer = frame_buf_uncompressed;
        m_sbc_enc_params.pu8Packet     = frame_buf_compressed;

        SBC_Encoder(&m_sbc_enc_params);

        compressed_frame_size = m_sbc_enc_params.u16PacketLength;
        encoder_timer_sum += hftimer_stop();
        m_audio_measure_cpu_usage_end(&m_encoding_cpu_gauge);
        nrf_gpio_pin_clear(NRF_GPIO_PIN_MAP(0, 2));

        if (compressed_frame_size == 0)
        {
            NRF_LOG_ERROR("Frame size 0");
        }
        if (compressed_frame_size > SBC_MAX_FRAME_SIZE)
        {
            NRF_LOG_ERROR("Frame size too large: %d vs %d", compressed_frame_size, SBC_MAX_FRAME_SIZE);
        }
        else if (compressed_frame_size < 0)
        {
            NRF_LOG_ERROR("opus_encode error: %d", compressed_frame_size);
        }

        APP_ERROR_CHECK_BOOL((compressed_frame_size > 0) && (compressed_frame_size <= SBC_MAX_FRAME_SIZE));

        m_audio_measure_bitrate(&m_bitrate_gauge, compressed_frame_size);

#if CONFIG_STACK_PROFILER_ENABLED
        encoder_stack_usage = stack_profiler_get_max_stack_usage();
        if (encoder_stack_usage > encoder_stack_usage_max)
        {
            encoder_stack_usage_max = encoder_stack_usage;
        }

    // Initialize stack profiler.
    stack_profiler_init();
#endif

        memset(frame_buf_uncompressed, 0, sizeof(frame_buf_uncompressed));
        // Decompress frame
        nrf_gpio_pin_set(NRF_GPIO_PIN_MAP(0, 2));
        m_audio_measure_cpu_usage_start(&m_decoding_cpu_gauge);
        hftimer_start();

        p_framedata = frame_buf_compressed;
        decode_status = OI_CODEC_SBC_DecodeFrame(&m_sbc_dec_params, (const OI_BYTE**)&p_framedata, &compressed_frame_size, frame_buf_uncompressed, &uncompressed_frame_size);

        // decompressed_frame_size = opus_decode(m_opus_decoder_state, frame_buf_compressed, compressed_frame_size, frame_buf_uncompressed, m_sbc_frame_size_samples, 0);
        decoder_timer_sum += hftimer_stop();
        m_audio_measure_cpu_usage_end(&m_decoding_cpu_gauge);
        nrf_gpio_pin_clear(NRF_GPIO_PIN_MAP(0, 2));

        APP_ERROR_CHECK_BOOL(decode_status == OI_OK);
        APP_ERROR_CHECK_BOOL(compressed_frame_size == 0); // Expect 0 bytes remaining to decode
        if (uncompressed_frame_size != (m_sbc_frame_size_samples * m_channel_count * sizeof(int16_t)))
        {
            NRF_LOG_ERROR("uncompressed_frame_size: %d", uncompressed_frame_size);
            APP_ERROR_CHECK_BOOL(uncompressed_frame_size == (m_sbc_frame_size_samples * m_channel_count * sizeof(int16_t)));
        }
        // APP_ERROR_CHECK_BOOL(decompressed_frame_size == m_sbc_frame_size_samples);

#if CONFIG_STACK_PROFILER_ENABLED
        decoder_stack_usage = stack_profiler_get_max_stack_usage();
        if (decoder_stack_usage > decoder_stack_usage_max)
        {
            decoder_stack_usage_max = decoder_stack_usage;
        }
#endif

#if 1

    static bool m_spi_initalized = false;
    if (!m_spi_initalized)
    {
        NRF_LOG_INFO("SPIM init");
        nrf_spim_pins_set(NRF_SPIM0, NRF_GPIO_PIN_MAP(1, 13), NRF_GPIO_PIN_MAP(1, 14), NRF_SPIM_PIN_NOT_CONNECTED);
        nrf_spim_frequency_set(NRF_SPIM0, NRF_SPIM_FREQ_8M);
        nrf_spim_configure(NRF_SPIM0, NRF_SPIM_MODE_0, NRF_SPIM_BIT_ORDER_MSB_FIRST);
        nrf_spim_enable(NRF_SPIM0);
        nrf_gpio_cfg_output(NRF_GPIO_PIN_MAP(1, 15));
        nrf_gpio_pin_set(NRF_GPIO_PIN_MAP(1, 15));
        m_spi_initalized = true;
    }
    nrf_gpio_pin_clear(NRF_GPIO_PIN_MAP(1, 15));
    nrf_spim_event_clear(NRF_SPIM0, NRF_SPIM_EVENT_ENDTX);
    nrf_spim_tx_buffer_set(NRF_SPIM0, (const uint8_t *)frame_buf_uncompressed, uncompressed_frame_size);
    NRF_SPIM0->TXD.MAXCNT = uncompressed_frame_size;
    nrf_spim_task_trigger(NRF_SPIM0, NRF_SPIM_TASK_START);
    while (!nrf_spim_event_check(NRF_SPIM0, NRF_SPIM_EVENT_ENDTX))
    {
        // Wait
    }
    nrf_gpio_pin_set(NRF_GPIO_PIN_MAP(1, 15));
//         APP_ERROR_CHECK_BOOL(m_sbc_frame_size_samples % 16 == 0);
//         NRF_LOG_INFO("FRAME IDX %d BEGIN", i * m_sbc_frame_size_samples);

//         for (int j = 0; j < m_sbc_frame_size_samples; j += 16)
//         {
//             NRF_LOG_INFO("%d, %d, %d, %d", frame_buf_uncompressed[j + 0], frame_buf_uncompressed[j + 1], frame_buf_uncompressed[j + 2], frame_buf_uncompressed[j + 3]);
//             NRF_LOG_INFO("%d, %d, %d, %d", frame_buf_uncompressed[j + 4], frame_buf_uncompressed[j + 5], frame_buf_uncompressed[j + 6], frame_buf_uncompressed[j + 7]);
//             NRF_LOG_INFO("%d, %d, %d, %d", frame_buf_uncompressed[j + 8], frame_buf_uncompressed[j + 9], frame_buf_uncompressed[j + 10], frame_buf_uncompressed[j + 11]);
//             NRF_LOG_INFO("%d, %d, %d, %d", frame_buf_uncompressed[j + 12], frame_buf_uncompressed[j + 13], frame_buf_uncompressed[j + 14], frame_buf_uncompressed[j + 15]);
//             // nrf_delay_us(10000);
//         }

//         while (NRF_LOG_PROCESS())
//         {
// #if CONFIG_CL_RTT_ENABLED
//           nrf_cli_process(&m_cli_rtt);
// #endif
// #if CONFIG_CL_UART_ENABLED
//             nrf_cli_process(&m_cli_uart);
// #endif
//         }
#endif
    }

    m_audio_bitrate_gauge_log(&m_bitrate_gauge, "\t- Bitrate");
    // m_audio_cpu_gauge_log(&m_encoding_cpu_gauge, "\t- Encoder");
    // m_audio_cpu_gauge_log(&m_decoding_cpu_gauge, "\t- Decoder");

    {
        uint32_t encoder_cpu_usage;
        uint32_t decoder_cpu_usage;

        encoder_cpu_usage = (uint32_t)((uint64_t)encoder_timer_sum * 1000 / (16 * 10 * (uint64_t)m_sbc_frame_size_us * (uint64_t) num_frames_in_sample));
        decoder_cpu_usage = (uint32_t)((uint64_t)decoder_timer_sum * 1000 / (16 * 10 * (uint64_t)m_sbc_frame_size_us * (uint64_t) num_frames_in_sample));

        NRF_LOG_INFO("Time used to encode %d frames: %d [1/16 us] = ~%d %% CPU usage", num_frames_in_sample, encoder_timer_sum, encoder_cpu_usage);
        NRF_LOG_INFO("Time used to decode %d frames: %d [1/16 us] = ~%d %% CPU usage", num_frames_in_sample, decoder_timer_sum, decoder_cpu_usage);
    }

#if CONFIG_STACK_PROFILER_ENABLED
    NRF_LOG_INFO("Maximum encoder stack usage: %u%% (%u out of %u bytes)",
              100ul * encoder_stack_usage_max / REAL_STACK_SIZE,
              encoder_stack_usage_max,
              REAL_STACK_SIZE);
    NRF_LOG_INFO("Maximum decoder stack usage: %u%% (%u out of %u bytes)",
              100ul * decoder_stack_usage_max / REAL_STACK_SIZE,
              decoder_stack_usage_max,
              REAL_STACK_SIZE);
#endif
}

/**
 * @brief Function for application main entry.
 */
int main(void)
{
    // Enable I-Cache.
    NRF_NVMC->ICACHECNF |= NVMC_ICACHECNF_CACHEEN_Msk;

    // Initialize Clock Management Driver.
    APP_ERROR_CHECK(nrf_drv_clock_init());

    // Start LFCLK.
    nrf_drv_clock_lfclk_request(NULL);
    nrf_drv_clock_hfclk_request(NULL);
    while (!nrf_drv_clock_hfclk_is_running())
    {
        __NOP();
    }

    nrf_gpio_cfg_output(NRF_GPIO_PIN_MAP(0, 2));
    nrf_gpio_pin_clear(NRF_GPIO_PIN_MAP(0, 2));

    app_timer_init();
    app_timer_resume();

#if CONFIG_LOG_ENABLED
    // Initialize SDK Logger.
    APP_ERROR_CHECK(NRF_LOG_INIT(app_timer_cnt_get));
#endif

#if CONFIG_CL_RTT_ENABLED
    /*
     * Start RTT console if debugger is connected. RTT console may be one of the logger backends
     * so it needs to be attached to logger as soon as possible.
     */
    if (CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk)
    {
#if CONFIG_CL_RTT_INTERFACE == CONFIG_CL_INTERFACE_LOGGER
        nrf_log_backend_rtt_init();
        int32_t rtt_backend_id = nrf_log_backend_add(&m_rtt_log_backend.backend,
                                                     (nrf_log_severity_t)CONFIG_LOG_RTT_SEVERITY);
        if (rtt_backend_id < 0)
        {
            ASSERT(false);
        }
        nrf_log_backend_enable(&m_rtt_log_backend.backend);
#endif

#if CONFIG_CL_RTT_INTERFACE == CONFIG_CL_INTERFACE_CLI
        APP_ERROR_CHECK(nrf_cli_init(&m_cli_rtt,
                                     NULL,
                                     true,
                                     CONFIG_CLI_RTT_LOG_BACKEND_ENABLED,
                                     (nrf_log_severity_t)(CONFIG_LOG_RTT_SEVERITY)));
        APP_ERROR_CHECK(nrf_cli_start(&m_cli_rtt));
#endif
    }
#endif

#if CONFIG_CL_UART_ENABLED
#if CONFIG_CL_RTT_INTERFACE == CONFIG_CL_INTERFACE_CLI
        nrf_drv_uart_config_t uart_config = NRF_DRV_UART_DEFAULT_CONFIG;
        uart_config.pseltxd = CONFIG_UART_TX_PIN;
        uart_config.pselrxd = CONFIG_UART_RX_PIN;
        uart_config.hwfc    = NRF_UART_HWFC_DISABLED;
        APP_ERROR_CHECK(nrf_cli_init(&m_cli_uart, &uart_config, true, true, NRF_LOG_SEVERITY_INFO));
        APP_ERROR_CHECK(nrf_cli_start(&m_cli_uart));
#endif
#endif

#if CONFIG_STACK_PROFILER_ENABLED
    // Initialize stack profiler.
    stack_profiler_init();
#endif

    if (REAL_STACK_SIZE < (CONFIG_STACK_SIZE - CONFIG_STACK_SIZE_GUARD))
    {
        NRF_LOG_WARNING("Stack size is lower than recommended (%d vs %d bytes)", REAL_STACK_SIZE, (CONFIG_STACK_SIZE - CONFIG_STACK_SIZE_GUARD));
    }
    else
    {
        NRF_LOG_INFO("Usable stack size: %d bytes", (CONFIG_STACK_SIZE - CONFIG_STACK_SIZE_GUARD));
    }

    sbc_param_update_and_init();

#if USE_WHITE_NOISE_SAMPLE
    union
    {
        uint32_t seed_u32;
        uint8_t  seed_u8[4];
    } seed;
    nrf_rng_event_clear(NRF_RNG_EVENT_VALRDY);
    nrf_rng_task_trigger(NRF_RNG_TASK_START);
    for (int i = 0; i < sizeof(seed.seed_u8); ++i)
    {
        while (!nrf_rng_event_get(NRF_RNG_EVENT_VALRDY)){}
        seed.seed_u8[i] = nrf_rng_random_value_get();
        nrf_rng_event_clear(NRF_RNG_EVENT_VALRDY);
    }
    nrf_rng_task_trigger(NRF_RNG_TASK_STOP);
    srand(seed.seed_u32);
    NRF_LOG_INFO("Rand seed: 0x%08X", seed.seed_u32);
#endif

    // run_loopback_test();

    while (true)
    {
        if (nrf_atomic_flag_clear_fetch(&m_run_test_flag) != 0)
        {
            run_loopback_test();
        }
        NRF_LOG_PROCESS();
#if CONFIG_CL_RTT_ENABLED
        nrf_cli_process(&m_cli_rtt);
#endif
#if CONFIG_CL_UART_ENABLED
        nrf_cli_process(&m_cli_uart);
#endif
    }
}

static void drv_audio_codec_set_cmd(nrf_cli_t const * p_cli, size_t argc, char **argv)
{
    if (nrf_cli_help_requested(p_cli))
    {
        nrf_cli_help_print(p_cli, NULL, 0);
        nrf_cli_fprintf(p_cli, NRF_CLI_NORMAL, "Usage:\r\n  %s <parameter> <value>\r\n", argv[0]);
        return;
    }

    if (argc >= 2)
    {
        nrf_cli_fprintf(p_cli, NRF_CLI_ERROR, "Unknown parameter '%s'!\r\n", argv[1]);
    }
    else
    {
        nrf_cli_fprintf(p_cli, NRF_CLI_ERROR, "Please specify parameter!\r\n");
    }
}

static void drv_audio_codec_info_cmd(nrf_cli_t const * p_cli, size_t argc, char **argv)
{
    if (nrf_cli_help_requested(p_cli))
    {
        nrf_cli_help_print(p_cli, NULL, 0);
        nrf_cli_fprintf(p_cli, NRF_CLI_NORMAL, "Usage:\r\n  %s\r\n", argv[0]);
        return;
    }

    nrf_cli_fprintf(p_cli, NRF_CLI_NORMAL, "Codec: %s\r\n",
                    (CONFIG_SBC_MODE == CONFIG_SBC_MODE_MSBC) ? "mSBC" : "SBC");

    nrf_cli_fprintf(p_cli, NRF_CLI_NORMAL, "\tBlocks:\t\t%u\r\n", m_sbc_blocks);
    nrf_cli_fprintf(p_cli, NRF_CLI_NORMAL, "\tSubbands:\t%u\r\n", m_sbc_subbands);
    nrf_cli_fprintf(p_cli, NRF_CLI_NORMAL, "\tBit Pool:\t%u\r\n", m_sbc_bitpool);
    nrf_cli_fprintf(p_cli, NRF_CLI_NORMAL, "\tAllocation:\t%s\r\n",
                    (SBC_ALLOCATION_METHOD == SBC_LOUDNESS) ? "Loudness" :
                    (SBC_ALLOCATION_METHOD == SBC_SNR)      ? "SNR" :
                    "Unknown");
}

static void drv_audio_codec_run_loopback(nrf_cli_t const * p_cli, size_t argc, char **argv)
{
    nrf_atomic_flag_set(&m_run_test_flag);
    // run_loopback_test();
}

// static void drv_audio_codec_set_bitrate_cmd(nrf_cli_t const * p_cli, size_t argc, char **argv)
// {
//     // bool vbr = m_opus_vbr;
//     char *p_str_end;
//     long bitrate;

//     if (nrf_cli_help_requested(p_cli))
//     {
//         nrf_cli_help_print(p_cli, NULL, 0);
//         nrf_cli_fprintf(p_cli, NRF_CLI_NORMAL, "Usage:\r\n  %s <bitrate> [vbr|cbr]\r\n", argv[0]);
//         return;
//     }

//     if ((argc < 2) || (argc > 3))
//     {
//         nrf_cli_fprintf(p_cli, NRF_CLI_ERROR, "Please specify bitrate and optional bitrate control mode!\r\n");
//         return;
//     }

//     // Parse value string.
//     if (!strcmp("auto", argv[1]))
//     {
//         // bitrate = OPUS_AUTO;
//     }
//     else
//     {
//         p_str_end = argv[1];
//         bitrate = strtol(argv[1], &p_str_end, 10) * 1000;

//         if ((*p_str_end != '\0') || (bitrate < 500) || (bitrate > 512000))
//         {
//             nrf_cli_fprintf(p_cli, NRF_CLI_ERROR, "Invalid bitrate!\r\n");
//             return;
//         }
//     }

//     // // Parse optional mode.
//     // if (argc >= 3)
//     // {
//     //     if (!strcmp("vbr", argv[2]))
//     //     {
//     //         vbr = true;
//     //     }
//     //     else if (!strcmp("cbr", argv[2]))
//     //     {
//     //         vbr = false;
//     //     }
//     //     else
//     //     {
//     //         nrf_cli_fprintf(p_cli, NRF_CLI_ERROR, "Invalid bitrate mode!\r\n");
//     //         return;
//     //     }
//     // }

//     // if (bitrate == OPUS_AUTO)
//     // {
//     //     // Select VBR mode if automatic bitrate management is selected.
//     //     vbr = true;
//     // }

//     // // Set new paramaters.
//     // m_opus_bitrate = bitrate;
//     // m_opus_vbr     = vbr;

//     // opus_param_update_and_init();
// }

static void drv_audio_codec_set_blocks_cmd(nrf_cli_t const * p_cli, size_t argc, char **argv)
{
    long blocks;
    char *p_str_end;

    // Check input parameters.
    if (nrf_cli_help_requested(p_cli))
    {
        nrf_cli_help_print(p_cli, NULL, 0);
        nrf_cli_fprintf(p_cli, NRF_CLI_NORMAL, "Usage:\r\n  %s <4, 8, 12, 16>\r\n", argv[0]);
        return;
    }

    if (argc != 2)
    {
        nrf_cli_fprintf(p_cli, NRF_CLI_ERROR, "Please specify block count! (4, 8, 12, 16)\r\n");
        return;
    }

    // Parse value string.
    p_str_end = argv[1];
    blocks = strtol(argv[1], &p_str_end, 10);
    if ((*p_str_end != '\0') || ((blocks != 4) && (blocks != 8) && (blocks != 12) && (blocks != 16)))
    {
        nrf_cli_fprintf(p_cli, NRF_CLI_ERROR, "Invalid block count! (4, 8, 12, 16)\r\n");
        return;
    }

    // Set new block count.
    m_sbc_blocks = blocks;

    sbc_param_update_and_init();
}

static void drv_audio_codec_set_subbands_cmd(nrf_cli_t const * p_cli, size_t argc, char **argv)
{
    long subbands;
    char *p_str_end;

    // Check input parameters.
    if (nrf_cli_help_requested(p_cli))
    {
        nrf_cli_help_print(p_cli, NULL, 0);
        nrf_cli_fprintf(p_cli, NRF_CLI_NORMAL, "Usage:\r\n  %s <4, 8>\r\n", argv[0]);
        return;
    }

    if (argc != 2)
    {
        nrf_cli_fprintf(p_cli, NRF_CLI_ERROR, "Please specify subband count! (4, 8)\r\n");
        return;
    }

    // Parse value string.
    p_str_end = argv[1];
    subbands = strtol(argv[1], &p_str_end, 10);
    if ((*p_str_end != '\0') || ((subbands != 4) && (subbands != 8)))
    {
        nrf_cli_fprintf(p_cli, NRF_CLI_ERROR, "Invalid subband count! (4, 8)\r\n");
        return;
    }

    // Set new block count.
    m_sbc_subbands = subbands;

    sbc_param_update_and_init();
}

static void drv_audio_codec_set_bitpool_cmd(nrf_cli_t const * p_cli, size_t argc, char **argv)
{
    long bitpool;
    char *p_str_end;

    // Check input parameters.
    if (nrf_cli_help_requested(p_cli))
    {
        nrf_cli_help_print(p_cli, NULL, 0);
        nrf_cli_fprintf(p_cli, NRF_CLI_NORMAL, "Usage:\r\n  %s <2-250>\r\n", argv[0]);
        return;
    }

    if (argc != 2)
    {
        nrf_cli_fprintf(p_cli, NRF_CLI_ERROR, "Please specify bitpool value! (2-250)\r\n");
        return;
    }

    // Parse value string.
    p_str_end = argv[1];
    bitpool = strtol(argv[1], &p_str_end, 10);
    if ((*p_str_end != '\0') || (bitpool < 2) || (bitpool > 250))
    {
        nrf_cli_fprintf(p_cli, NRF_CLI_ERROR, "Invalid bitpool value! (2-250)\r\n");
        return;
    }

    // Set new block count.
    m_sbc_bitpool = bitpool;

    sbc_param_update_and_init();
}

static void drv_audio_codec_set_channels_cmd(nrf_cli_t const * p_cli, size_t argc, char **argv)
{
    long channels;
    char *p_str_end;

    // Check input parameters.
    if (nrf_cli_help_requested(p_cli))
    {
        nrf_cli_help_print(p_cli, NULL, 0);
        nrf_cli_fprintf(p_cli, NRF_CLI_NORMAL, "Usage:\r\n  %s <1-2>\r\n", argv[0]);
        return;
    }

    if (argc != 2)
    {
        nrf_cli_fprintf(p_cli, NRF_CLI_ERROR, "Please specify channel count! (1, 2)\r\n");
        return;
    }

    // Parse value string.
    p_str_end = argv[1];
    channels = strtol(argv[1], &p_str_end, 10);
    if ((*p_str_end != '\0') || (channels < 0) || (channels > 2))
    {
        nrf_cli_fprintf(p_cli, NRF_CLI_ERROR, "Invalid channel selection!\r\n");
        return;
    }

    m_channel_count = channels;

    sbc_param_update_and_init();
}

static void m_audio_cmd(nrf_cli_t const * p_cli, size_t argc, char **argv)
{
    if (nrf_cli_help_requested(p_cli))
    {
        nrf_cli_help_print(p_cli, NULL, 0);
        nrf_cli_fprintf(p_cli, NRF_CLI_NORMAL, "Usage:\r\n  %s <subcommand> ...\r\n", argv[0]);
        return;
    }

    if (argc >= 2)
    {
        nrf_cli_fprintf(p_cli, NRF_CLI_ERROR, "Unknown subcommand '%s'!\r\n", argv[1]);
    }
    else
    {
        nrf_cli_fprintf(p_cli, NRF_CLI_ERROR, "Specify a subcommand!\r\n");
    }
}

NRF_CLI_CREATE_STATIC_SUBCMD_SET(drv_audio_codec_set_subcmds)
{
    NRF_CLI_CMD(blocks,    NULL,   "set codec blocks",    drv_audio_codec_set_blocks_cmd),
    NRF_CLI_CMD(subbands,    NULL,   "set codec subbands",    drv_audio_codec_set_subbands_cmd),
    NRF_CLI_CMD(bitpool,    NULL,   "set codec bitpool",    drv_audio_codec_set_bitpool_cmd),
    // NRF_CLI_CMD(complexity, NULL,   "set codec complexity", drv_audio_codec_set_complexity_cmd),
    NRF_CLI_CMD(channels, NULL,     "set channel count",    drv_audio_codec_set_channels_cmd),
    { NULL }
};

static const nrf_cli_static_entry_t drv_audio_codec_subcmds_table[] =
{
    NRF_CLI_CMD(info,   NULL,                           "print information about codec",    drv_audio_codec_info_cmd),
    NRF_CLI_CMD(set,    &drv_audio_codec_set_subcmds,   "set codec parameter",              drv_audio_codec_set_cmd),
    NRF_CLI_CMD(run,    NULL,                           "run loopback test",                drv_audio_codec_run_loopback),
    { NULL }
};

const nrf_cli_cmd_entry_t drv_audio_codec_subcmds =
{
    .is_dynamic = false,
    .u.p_static = drv_audio_codec_subcmds_table,
};

NRF_CLI_CREATE_STATIC_SUBCMD_SET(m_audio_subcmds)
{
    NRF_CLI_CMD(codec,  &drv_audio_codec_subcmds,   "show or configure audio codec parameters",     m_audio_cmd),
    {NULL}
};

NRF_CLI_CMD_REGISTER(audio, &m_audio_subcmds, "show or configure audio settings", m_audio_cmd);
/** @} */
