// Copyright 2022-2026 XMOS LIMITED.
// This Software is subject to the terms of the XMOS Public Licence: Version 1.
#include <string.h>
#include <stdlib.h>

#include "aec.h"
#include "adec.h"

#include "pipeline_state.h"


static void aec_switch_configuration(pipeline_state_t *state, aec_conf_t *conf)
{
    aec_init(&state->aec_state,
            conf->num_y_channels, conf->num_x_channels,
            conf->num_main_filt_phases, conf->num_shadow_filt_phases, conf->tdist);
}


void pipeline_init(pipeline_state_t *state, aec_conf_t *de_conf, aec_conf_t *non_de_conf, adec_config_t *adec_config) {
    memset(state, 0, sizeof(pipeline_state_t));
    state->delay_estimator_enabled = 0;
    state->adec_requested_delay_samples = 0;
    state->ref_active_threshold =  f64_to_float_s32(pow(10, -60/20.0));

    // Initialise default delay values
    delay_buffer_init(&state->delay_state, 0/*Initialise with 0 delay_samples*/);

    memcpy(&state->aec_de_mode_conf, de_conf, sizeof(aec_conf_t));
    memcpy(&state->aec_non_de_mode_conf, non_de_conf, sizeof(aec_conf_t));

    adec_init(&state->adec_state, adec_config);
    aec_switch_configuration(state, &state->aec_non_de_mode_conf);
}

static inline void get_delayed_frame(
        int32_t (*input_y_data)[AP_FRAME_ADVANCE],
        int32_t (*input_x_data)[AP_FRAME_ADVANCE],
        delay_buf_state_t *delay_state)
{
    int num_channels = (delay_state->delay_samples) > 0 ? AP_MAX_Y_CHANNELS : AP_MAX_X_CHANNELS;
    if (delay_state->delay_samples >= 0) {/** Requested Mic delay +ve => delay mic*/
        for(int ch=0; ch<num_channels; ch++) {
            for(int i=0; i<AP_FRAME_ADVANCE; i++) {
                get_delayed_sample(delay_state, &input_y_data[ch][i], ch);
            }
        }
    }
    else if (delay_state->delay_samples < 0) {/* Requested Mic delay negative => advance mic which can't be done, so delay reference*/
        for(int ch=0; ch<num_channels; ch++) {
            for(int i=0; i<AP_FRAME_ADVANCE; i++) {
                get_delayed_sample(delay_state, &input_x_data[ch][i], ch);
            }
        }
    }
    return;
}

int framenum = 0;
void pipeline_process_frame(pipeline_state_t *state,
        int32_t (*output_data)[AP_FRAME_ADVANCE],
        int32_t (*input_y_data)[AP_FRAME_ADVANCE],
        int32_t (*input_x_data)[AP_FRAME_ADVANCE]
        )
{
    /** Get delayed frame*/
    delay_buf_state_t *delay_state_ptr = &state->delay_state;
    get_delayed_frame(
            input_y_data,
            input_x_data,
            delay_state_ptr
            );

    /** Switch AEC config if needed. We're doing it at the beginning of current frame instead of end of previous
     * frame to be able to read relevant outputs from the state in the test_wav code for logging. Switching at the
     * end of last frame would have reset the state.*/
    if (state->adec_output_delay_estimator_enabled_flag && !state->delay_estimator_enabled) {
        /** Now that a AEC -> DE change has been requested, reset force_de_cycle_trigger in case this transition is
         * requested as a result of force_de_cycle_trigger being set*/
        state->adec_state.adec_config.force_de_cycle_trigger = 0;

        // Initialise AEC for delay estimation config
        aec_switch_configuration(state, &state->aec_de_mode_conf);
        state->aec_state.main_state.shared_state->config_params.coh_mu_conf.adaption_config = AEC_ADAPTION_FORCE_ON;
        //state->aec_state.main_state.shared_state->config_params.coh_mu_conf.force_adaption_mu_q30 = fixed_mu_delay_est_mode;
        state->delay_estimator_enabled = 1;
    } else if ((!state->adec_output_delay_estimator_enabled_flag && state->delay_estimator_enabled)) {
        // Start AEC for normal aec config
        aec_switch_configuration(state, &state->aec_non_de_mode_conf);
        state->delay_estimator_enabled = 0;
    }

    //printf("frame %d\n",framenum);

    /** AEC*/
    int32_t aec_output_shadow[AP_MAX_Y_CHANNELS][AP_FRAME_ADVANCE];
    // Writing main filter output to output_data directly
    aec_process_frame(&state->aec_state, output_data, aec_output_shadow, input_y_data, input_x_data);

    /** Delay estimator*/
    adec_input_t adec_in;
    adec_estimate_delay(
            &adec_in.from_de,
            state->aec_state.main_state.H_hat[0],
            state->aec_state.main_state.num_phases
            );

    /** ADEC*/
    // Create input to ADEC from AEC
    adec_in.from_aec.y_ema_energy_ch0 = state->aec_state.main_state.shared_state->y_ema_energy[0];
    adec_in.from_aec.error_ema_energy_ch0 = state->aec_state.main_state.error_ema_energy[0];
    adec_in.from_aec.shadow_flag_ch0 = state->aec_state.main_state.shared_state->shadow_filter_params.shadow_flag[0];
    adec_in.far_end_active_flag = state->aec_state.main_state.shared_state->ref_active_flag;

    // Log current mode for printing later
#ifdef ENABLE_ADEC_DEBUG_PRINTS
    adec_mode_t old_mode = state->adec_state.mode;
#endif

    // Call ADEC
    adec_output_t adec_output;
    adec_process_frame(
            &state->adec_state,
            &adec_output,
            &adec_in
            );

    //** Reset AEC state if needed*/
    if(adec_output.reset_aec_flag) {
        aec_reset_state(&state->aec_state);
    }

    /** Update delay samples if there's a delay change requested by ADEC*/
    if(adec_output.delay_change_request_flag == 1){
        // Update delay_buffer delay_samples with mic delay requested by adec
        update_delay_samples(&state->delay_state, adec_output.requested_mic_delay_samples);
        for(int ch=0; ch<AP_MAX_Y_CHANNELS; ch++) {
            reset_partial_delay_buffer(&state->delay_state, ch);
        }
#ifdef ENABLE_ADEC_DEBUG_PRINTS
        printf("!!ADEC STATE CHANGE!! Frame: %d old: %s new: %s\n", framenum, old_mode?"DE":"AEC", state->adec_state.mode?"DE":"AEC");

        printf("AP Setting MIC delay to: %ld\n", state->delay_state.delay_samples);
#endif
    }

    /** Overwrite output with mic input if delay estimation enabled*/
    if (state->delay_estimator_enabled) {
        // Send the current frame unprocessed
        for(int ch=0; ch<AP_MAX_Y_CHANNELS; ch++) {
            for(int i=0; i<AP_FRAME_ADVANCE; i++) {
                output_data[ch][i] = input_y_data[ch][i];
            }
        }
    }
    state->adec_output_delay_estimator_enabled_flag = adec_output.delay_estimator_enabled_flag; //Keep a copy so we can use it in next frame
    state->de_output_measured_delay_samples = adec_in.from_de.measured_delay_samples;


    //For logging for test purposes
    /* If a delay change is requested and this request is not accompanied by a AEC -> DE change, update
     * state->adec_requested_delay_samples(bits [0:30]) and toggle last bit (bit31) of
     * state->adec_requested_delay_samples to indicate to the test that a delay change event has occured*/
    if((adec_output.delay_change_request_flag == 1) && (adec_output.delay_estimator_enabled_flag == 0)){
        /* Update MSB 31 bits of state->adec_requested_delay_samples with the new delay. Toggle LSB bit of
         * state->adec_requested_delay_samples to indicate that a new delay (even if it happens to be same as the
         * existing delay) is requested by adec*/
         unsigned new_bottom_bit = (state->adec_requested_delay_samples & 0x1) ^ 0x1;
         state->adec_requested_delay_samples = adec_output.requested_delay_samples_debug;
         state->adec_requested_delay_samples = (state->adec_requested_delay_samples & 0xfffffffe) | new_bottom_bit;
    }

    framenum++;
}
