/***********************************************************************************************************************
 * Copyright [2020 - 2025] Renesas Electronics Corporation and/or its affiliates.  All Rights Reserved.
 *
 * This software and documentation are supplied by Renesas Electronics America Inc. and may only be used with products
 * of Renesas Electronics Corp. and its affiliates ("Renesas").  No other uses are authorized.  Renesas products are
 * sold pursuant to Renesas terms and conditions of sale.  Purchasers are solely responsible for the selection and use
 * of Renesas products and Renesas assumes no liability.  No license, express or implied, to any intellectual property
 * right is granted by Renesas. This software is protected under all applicable laws, including copyright laws. Renesas
 * reserves the right to change or discontinue this software and/or this documentation. THE SOFTWARE AND DOCUMENTATION
 * IS DELIVERED TO YOU "AS IS," AND RENESAS MAKES NO REPRESENTATIONS OR WARRANTIES, AND TO THE FULLEST EXTENT
 * PERMISSIBLE UNDER APPLICABLE LAW, DISCLAIMS ALL WARRANTIES, WHETHER EXPLICITLY OR IMPLICITLY, INCLUDING WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND NONINFRINGEMENT, WITH RESPECT TO THE SOFTWARE OR
 * DOCUMENTATION.  RENESAS SHALL HAVE NO LIABILITY ARISING OUT OF ANY SECURITY VULNERABILITY OR BREACH.  TO THE MAXIMUM
 * EXTENT PERMITTED BY LAW, IN NO EVENT WILL RENESAS BE LIABLE TO YOU IN CONNECTION WITH THE SOFTWARE OR DOCUMENTATION
 * (OR ANY PERSON OR ENTITY CLAIMING RIGHTS DERIVED FROM YOU) FOR ANY LOSS, DAMAGES, OR CLAIMS WHATSOEVER, INCLUDING,
 * WITHOUT LIMITATION, ANY DIRECT, CONSEQUENTIAL, SPECIAL, INDIRECT, PUNITIVE, OR INCIDENTAL DAMAGES; ANY LOST PROFITS,
 * OTHER ECONOMIC DAMAGE, PROPERTY DAMAGE, OR PERSONAL INJURY; AND EVEN IF RENESAS HAS BEEN ADVISED OF THE POSSIBILITY
 * OF SUCH LOSS, DAMAGES, CLAIMS OR COSTS.
 **********************************************************************************************************************/

#include "hal_data.h"

FSP_CPP_HEADER
void R_BSP_WarmStart(bsp_warm_start_event_t event);
FSP_CPP_FOOTER

//////////////////////////////////////////////////////////////////////////////////
///////////////// USER DEFINED ///////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////

#define DCDS_MODE       (1)       // Must be enabled to use Live Monitor or DST
#define INFERENCE_MODE  (1)

#define FRAME_BUFFER_SIZE   (4096)   // Needs to match FSP data collector frame buffer size and AI model window length
#define SAMPLE_RATE         (4096)   // Calculates the period for the BLCK timer according to given SR

#define RAI_MODEL_PREFIX baby_cry_alp_v1_          //Update this when changing the AI model

#define SHIFT_AMOUNT    (15)
#define DC_OFFSET       (0)

#define USE_FIR                 (0)
#define DECIMATION_FACTOR       (4)
#define FIR_NUM_TAPS            (64U)

#if USE_FIR
static const int16_t fir_coeff[FIR_NUM_TAPS] = {
-56, 193, 134, 89, 9, -93, -170, -178, -94, 59, 219, 302, 249, 55, -211, -428,
-475, -288, 91, 514, 775, 701, 240, -482, -1177, -1482, -1091, 111, 1973, 4097, 5949, 7026,
7026, 5949, 4097, 1973, 111, -1091, -1482, -1177, -482, 240, 701, 775, 514, 91, -288, -475,
-428, -211, 55, 249, 302, 219, 59, -94, -178, -170, -93, 9, 89, 134, 193, -56,
};
#endif


///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////


#define GETCAT_INNER(b) get_##b##model
#define GETCAT(b) GETCAT_INNER(b)
#define RAI_GET_MODEL GETCAT(RAI_MODEL_PREFIX)    //get_<RAI_MODEL_PREFIX>model

#define PREDICTCAT_INNER(x) x##predict
#define PREDICTCAT(x) PREDICTCAT_INNER(x)
#define RAI_PREDICT PREDICTCAT(RAI_MODEL_PREFIX)  //<RAI_MODEL_PREFIX>predict

#define STR(x) #x
#define INCLUDE_MODEL_INNER(x) STR(x##model.h)
#define INCLUDE_MODEL(x) INCLUDE_MODEL_INNER(x)   //"<RAI_MODEL_PREFIX>model.h"

#include INCLUDE_MODEL(RAI_MODEL_PREFIX)
#include "RealityAI.h"

#if USE_FIR
#include "dsp/filtering_functions.h"

#define DMIC_BUF_SIZE FRAME_BUFFER_SIZE*DECIMATION_FACTOR

#define FIR_BUF_LEN (FIR_NUM_TAPS + DMIC_BUF_SIZE - 1)
static q15_t cmsis_state[FIR_BUF_LEN];
static arm_fir_decimate_instance_q15 S = {
   .M       = DECIMATION_FACTOR,
   .numTaps = FIR_NUM_TAPS,
   .pCoeffs = fir_coeff,
   .pState  = cmsis_state
};
#else

#define DMIC_BUF_SIZE FRAME_BUFFER_SIZE

#endif


static uint32_t dmic_buf[2][DMIC_BUF_SIZE];
static volatile uint8_t dmic_idx;

static volatile bool dmic_done;
static volatile bool dmic_err;
static volatile bool processing_pong = false;

static uint32_t g_events = 0;
static uint32_t g_err_count = 0;
static bool g_dc_callback = false;
static rai_data_collector_callback_args_t g_callback_args;
static rai_data_collector_frame_buffer_t g_frame_buf[8];

static int16_t *data_feed;

extern int RealityAI_get_monitor_data_size(struct rai_model_struct* model);
extern struct ai_monitor* RealityAI_get_monitor_data(struct rai_model_struct* model);

static int pred = NULL;
float pred_scores[2] = {0};
float baby_cry = 0;

void hal_entry(void)
{
    fsp_err_t err;

    /* Initialize Reality AI model */
#if INFERENCE_MODE
    struct rai_model_struct* p_model = RAI_GET_MODEL();
#endif

    /* Initialize ELC peripheral */
    err = R_ELC_Open(&g_elc_ctrl, &g_elc_cfg);
    if (FSP_SUCCESS != err)
    {
        __BKPT(0);
    }

    /* Enabled configured ELC links */
    err = R_ELC_Enable(&g_elc_ctrl);
    if (FSP_SUCCESS != err)
    {
        __BKPT(0);
    }

    /* Initialize timer used to generate I2S BLCK signal */
    err = R_GPT_Open(&g_timer_bclk_ctrl, &g_timer_bclk_cfg);
    if (FSP_SUCCESS != err)
    {
        __BKPT(0);
    }

    /* Set the period for the BLCK signal  */
    uint32_t pclkd_freq_hz = R_FSP_SystemClockHzGet(FSP_PRIV_CLOCK_PCLKD) >> g_timer_bclk_cfg.source_div;
    uint32_t period_counts = (uint32_t) (((uint64_t) pclkd_freq_hz) / (SAMPLE_RATE*64));
    err = R_GPT_PeriodSet(&g_timer_bclk_ctrl, period_counts);
    if (FSP_SUCCESS != err)
    {
        __BKPT(0);
    }

    /* Initialize timer used to generate I2S WS signal */
    err = R_GPT_Open(&g_timer_ws_ctrl, &g_timer_ws_cfg);
    if (FSP_SUCCESS != err)
    {
        __BKPT(0);
    }

    /* Set initial counter value before the cycle start for SPI to register falling edge */
    err = R_GPT_CounterSet(&g_timer_ws_ctrl, g_timer_ws_cfg.period_counts - 1);
    if (FSP_SUCCESS != err)
    {
        __BKPT(0);
    }

    /* Enable the I2S WS timer (counting will only start after I2S BLCK is enabled) */
    err = R_GPT_Start(&g_timer_ws_ctrl);
    if (FSP_SUCCESS != err)
    {
        __BKPT(0);
    }

    /* Initialize SPI perpiheral used to receive I2S data */
    err = R_SPI_Open(&g_spi0_ctrl, &g_spi0_cfg);
    if (FSP_SUCCESS != err)
    {
        __BKPT(0);
    }

    /* Start the I2S BCLK clock */
    err = R_GPT_Start(&g_timer_bclk_ctrl);
    if (FSP_SUCCESS != err)
    {
        __BKPT(0);
    }

    /* Set up the initial I2S read */
    err = R_SPI_Read(&g_spi0_ctrl, dmic_buf[dmic_idx], DMIC_BUF_SIZE, SPI_BIT_WIDTH_32_BITS);
    if (FSP_SUCCESS != err)
    {
        __BKPT(0);
    }
#if DCDS_MODE
    /* Initialize the Data Shipper */
    err = RM_RAI_DATA_SHIPPER_Open(&g_rai_data_shipper0_ctrl, &g_rai_data_shipper0_cfg);
    if (FSP_SUCCESS != err){
        __BKPT(0);
    }


    /* Initialize the Data Collector */
    err = RM_RAI_DATA_COLLECTOR_Open(&g_rai_data_collector0_ctrl, &g_rai_data_collector0_cfg);
    if (FSP_SUCCESS != err){
        __BKPT(0);
    }
#endif



    while (1)
    {
        /* Wait for interrupt & check for event */
        while ((false == dmic_done) && (false == dmic_err))
            __WFI();

        if (true == dmic_err)
        {
            dmic_err = false;

            /* Restart SPI peripheral to clear the underrun error state */
            R_SPI_Close(&g_spi0_ctrl);
            R_SPI_Open(&g_spi0_ctrl, &g_spi0_cfg);
            do
            {
                /* Repeat this request if it fails */
                err = R_SPI_Read(&g_spi0_ctrl, dmic_buf, DMIC_BUF_SIZE, SPI_BIT_WIDTH_32_BITS);
            }
            while (FSP_SUCCESS != err);
        }

        else // (true == dmic_done)
        {
            dmic_done = false;
            processing_pong = true;

            data_feed = (int16_t *)dmic_buf[dmic_idx ^ 1];

            for (int i = 0; i < DMIC_BUF_SIZE; i++)
                        {
                        data_feed[2*i] = (int16_t)((int32_t)(dmic_buf[dmic_idx ^ 1][i]<<1)>>SHIFT_AMOUNT)-DC_OFFSET;
                        data_feed[i] = data_feed[2*i];
                        }
#if USE_FIR
            int16_t* filtered_buffer = data_feed + DMIC_BUF_SIZE;
            arm_fir_decimate_q15(&S, (int16_t*)data_feed, (int16_t*)filtered_buffer, DMIC_BUF_SIZE);

            data_feed = filtered_buffer;
#endif

#if DCDS_MODE
            /* Write data feed to Data Collector */
            err = RM_RAI_DATA_COLLECTOR_ChannelWrite(&g_rai_data_collector0_ctrl, 0, data_feed, FRAME_BUFFER_SIZE);
            if (FSP_SUCCESS != err){
                __BKPT(0);
            }

            /* Write data to Data Shipper */
            rai_data_shipper_write_params_t arg;
            arg.events = g_events;
#if INFERENCE_MODE
            // For AI Live Monitor
            arg.diagnostic_data_len = (uint8_t)RealityAI_get_monitor_data_size(p_model);
            arg.p_diagnostic_data = (uint8_t*)RealityAI_get_monitor_data(p_model);
#else
            arg.diagnostic_data_len = 0;
            arg.p_diagnostic_data = NULL;
#endif //if Inference Mode
            arg.p_sensor_data = &g_callback_args;
            err = RM_RAI_DATA_SHIPPER_Write(&g_rai_data_shipper0_ctrl, &arg);

#endif //if DCDS Mode


#if INFERENCE_MODE
            //Assumes that window length of RAI model is equal to INFERENCE_BUF
            pred = RAI_PREDICT(data_feed);
            RealityAI_get_class_scores(pred_scores, DMIC_BUF_SIZE, 1, p_model);
            if (pred  == 1)
            {

                baby_cry = 1;
                // pred == 1 is glass break
                R_IOPORT_PinWrite(&g_ioport_ctrl, BSP_IO_PORT_05_PIN_00, 1);
                // blue on
                R_IOPORT_PinWrite(&g_ioport_ctrl, BSP_IO_PORT_01_PIN_13, 0);
             }
             else if(pred == 2)
             {
                baby_cry = 2;
                // pred == 2 is everything else
                R_IOPORT_PinWrite(&g_ioport_ctrl, BSP_IO_PORT_01_PIN_13, 1);
                // red on
                R_IOPORT_PinWrite(&g_ioport_ctrl, BSP_IO_PORT_05_PIN_00, 0);

             }

            p_model->results[1] = baby_cry;
            //p_model->results[1] = RealityAI_smooth(scream, p_model);
#endif
            processing_pong = false;
        }
    }
}

void g_spi0_cb(spi_callback_args_t * p_args)
{
    if (SPI_EVENT_TRANSFER_COMPLETE == p_args->event)
    {
        if(processing_pong){
            // If we get here there is a problem.
            // We shouldn't be flipping the dmic buffer index while still processing
            // Hopefully all processing is fast enough that this never happens.
            __BKPT(0);
        }

        /* Change index of the active write buffer */
        dmic_idx ^= 1;

        /* Start subsequent I2S read */
        R_SPI_Read(&g_spi0_ctrl, dmic_buf[dmic_idx], DMIC_BUF_SIZE, SPI_BIT_WIDTH_32_BITS);

        dmic_done = true;
    }

    else if (SPI_EVENT_ERR_MODE_UNDERRUN == p_args->event)
    {
        /* SPI peripheral wasn't ready when data was sent */
        dmic_err = true;
    }

    else
    {}
}


void rm_comms_uart_callback(uart_callback_args_t *p_args)
{

}

void rai_data_collector0_callback(const rai_data_collector_callback_args_t *p_args)
{
    g_callback_args.frame_buf_len = p_args->frame_buf_len;
    g_callback_args.frames = p_args->frames;
    g_callback_args.instance_id = p_args->instance_id;
    for(uint8_t i = 0; i < p_args->frames; i++)
    {
        g_frame_buf[i] = p_args->p_frame_buf[i];
    }
    g_callback_args.p_frame_buf = g_frame_buf;

    g_dc_callback = true;
}

void rai_data_collector0_error_callback(const rai_data_collector_error_callback_args_t *p_args)
{
    g_events |= p_args->event;
}
void rai_data_shipper0_callback(rai_data_shipper_callback_args_t *p_args)
{
    if(p_args->result != FSP_SUCCESS)
    {
        g_err_count++;
    }
    g_rai_data_collector0.p_api->bufferRelease(&g_rai_data_collector0_ctrl);
}


void R_BSP_WarmStart(bsp_warm_start_event_t event)
{
    if (BSP_WARM_START_POST_C == event)
    {
        /* C runtime environment and system clocks are setup. */

        /* Configure pins. */
        R_IOPORT_Open (&g_ioport_ctrl, g_ioport.p_cfg);
    }
}
