#include "audio.h"
#include "cmsis_os2.h"
#include "standard_output/standard_output.h"

#include <pdm2pcm_glo.h>
#include <stdbool.h>
#include <stm32h7xx_hal.h>
#include <string.h>

static SAI_HandleTypeDef sai;
static DMA_HandleTypeDef dma;
static uint8_t rx_buf[ACQUISITION_LENGTH * 4] __attribute__((section(".SAISection"))) = {0};
static PDM_Filter_Handler_t pdm_fh;
static PDM_Filter_Config_t pdm_fc;
static osThreadId_t th;
static osEventFlagsId_t flags;

#define AUDIO_RECEPTION_DONE (0x01)

void HAL_SAI_MspInit(SAI_HandleTypeDef *hsai) {
    __HAL_RCC_BDMA_CLK_ENABLE();

    // link BDMA0 to SAI4 RX
    dma.Instance = BDMA_Channel0;
    dma.Init.Request = BDMA_REQUEST_SAI4_A;
    dma.Init.Direction = DMA_PERIPH_TO_MEMORY;
    dma.Init.PeriphInc = DMA_PINC_DISABLE;
    dma.Init.MemInc = DMA_MINC_ENABLE;
    dma.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
    dma.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
    dma.Init.Mode = DMA_NORMAL;
    dma.Init.Priority = DMA_PRIORITY_LOW;
    HAL_DMA_Init(&dma);

    __HAL_LINKDMA(&sai, hdmarx, dma);
    //__HAL_LINKDMA(&sai, hdmatx, dma);

    HAL_NVIC_SetPriority(BDMA_Channel0_IRQn, 12, 0);
    HAL_NVIC_EnableIRQ(BDMA_Channel0_IRQn);
}

void BDMA_Channel0_IRQHandler() {
    HAL_DMA_IRQHandler(&dma);
}

void SAI4_IRQHandler() {
    HAL_SAI_IRQHandler(&sai);
}

static void sai_rx_cplt_cb(SAI_HandleTypeDef *sai) {
    osEventFlagsSet(flags, AUDIO_RECEPTION_DONE);
}

static void sai_init() {
    __HAL_RCC_SAI4_CLK_ENABLE();
    __HAL_RCC_SAI4_CLKAM_ENABLE();
    __HAL_RCC_GPIOE_CLK_ENABLE();

    // initialize pins
    GPIO_InitTypeDef gpio;
    gpio.Pin = GPIO_PIN_5;
    gpio.Mode = GPIO_MODE_AF_PP;
    gpio.Pull = GPIO_NOPULL;
    gpio.Speed = GPIO_SPEED_FREQ_LOW;
    gpio.Alternate = GPIO_AF10_SAI4;
    HAL_GPIO_Init(GPIOE, &gpio);

    gpio.Pin = GPIO_PIN_4;
    gpio.Mode = GPIO_MODE_AF_PP;
    gpio.Pull = GPIO_NOPULL;
    gpio.Speed = GPIO_SPEED_FREQ_LOW;
    gpio.Alternate = GPIO_AF10_SAI4;
    HAL_GPIO_Init(GPIOE, &gpio);

    // initialize SAI4
    memset(&sai, 0, sizeof(SAI_HandleTypeDef));
    sai.Instance = SAI4_Block_A;
    sai.Init.Protocol = SAI_FREE_PROTOCOL;
    sai.Init.AudioMode = SAI_MODEMASTER_RX;
    sai.Init.DataSize = SAI_DATASIZE_8;
    sai.Init.FirstBit = SAI_FIRSTBIT_MSB;
    sai.Init.ClockStrobing = SAI_CLOCKSTROBING_RISINGEDGE;
    sai.Init.Synchro = SAI_ASYNCHRONOUS;
    sai.Init.OutputDrive = SAI_OUTPUTDRIVE_DISABLED;
    sai.Init.NoDivider = SAI_MASTERDIVIDER_DISABLE;
    sai.Init.MckOverSampling = SAI_MCK_OVERSAMPLING_DISABLE;
    sai.Init.FIFOThreshold = SAI_FIFOTHRESHOLD_FULL;
    sai.Init.AudioFrequency = SAI_AUDIO_FREQUENCY_16K;
    sai.Init.MonoStereoMode = SAI_STEREOMODE;
    sai.Init.CompandingMode = SAI_NOCOMPANDING;
    sai.Init.PdmInit.Activation = ENABLE;
    sai.Init.PdmInit.MicPairsNbr = 2; // 2
    sai.Init.PdmInit.ClockEnable = SAI_PDM_CLOCK2_ENABLE;
    sai.FrameInit.FrameLength = 32; // 32
    sai.FrameInit.ActiveFrameLength = 1;
    sai.FrameInit.FSDefinition = SAI_FS_STARTFRAME;
    sai.FrameInit.FSPolarity = SAI_FS_ACTIVE_HIGH;
    sai.FrameInit.FSOffset = SAI_FS_FIRSTBIT;
    sai.SlotInit.FirstBitOffset = 0;
    sai.SlotInit.SlotSize = SAI_SLOTSIZE_DATASIZE;
    sai.SlotInit.SlotNumber = 4;      // 4
    sai.SlotInit.SlotActive = 0x000F; // F
    if (HAL_SAI_Init(&sai) != HAL_OK) {
        MSG("Hiba a SAI4 inicializációjánál!\n");
    }

    HAL_SAI_RegisterCallback(&sai, HAL_SAI_RX_COMPLETE_CB_ID, sai_rx_cplt_cb);

    SAI_Block_TypeDef *inst = sai.Instance;
    MODIFY_REG(inst->CR1, SAI_xCR1_MCKDIV, 1 << SAI_xCR1_MCKDIV_Pos);

    //__HAL_SAI_ENABLE(&sai);

    HAL_NVIC_SetPriority(SAI4_IRQn, 12, 0);
    HAL_NVIC_EnableIRQ(SAI4_IRQn);

    // clear sample buffers
    memset(rx_buf, 0, sizeof(uint32_t) * ACQUISITION_LENGTH);
}

// initialize PDM2PCM library
static void pdm2pcm_init() {
    pdm_fh.bit_order = PDM_FILTER_BIT_ORDER_LSB;
    pdm_fh.endianness = PDM_FILTER_ENDIANNESS_LE;
    pdm_fh.high_pass_tap = 2104533974;
    pdm_fh.in_ptr_channels = 1;
    pdm_fh.out_ptr_channels = 1;
    uint32_t status;
    if ((status = PDM_Filter_Init(&pdm_fh)) != 0) {
        MSG("PDM init error: %u\n", status);
    }

    pdm_fc.decimation_factor = PDM_FILTER_DEC_FACTOR_64;
    pdm_fc.output_samples_number = 1024;
    pdm_fc.mic_gain = 0;
    if ((status = PDM_Filter_setConfig(&pdm_fh, &pdm_fc)) != 0) {
        MSG("PDM config error: %u\n", status);
    }
}

static uint8_t input[ACQUISITION_LENGTH];
static int16_t output[ACQUISITION_LENGTH];

static void thread_audio(void *arg) {
    while (true) {
        uint32_t signals = osEventFlagsWait(flags, AUDIO_RECEPTION_DONE, osFlagsWaitAny, osWaitForever);
        if (signals & AUDIO_RECEPTION_DONE) {
            // initialize the filter algorithm
            uint32_t status;

            // extract CH2R samples
            for (uint16_t i = 0; i < ACQUISITION_LENGTH; i++) {
                input[i] = rx_buf[2 + 4 * i];
            }

            // filter the samples
            if ((status = PDM_Filter(input, output, &pdm_fh)) != 0) {
                MSG("PDM filter error: %u\n", status);
            }
        }
    }
}

void thread_init() {
    // create event flags
    flags = osEventFlagsNew(NULL);

    // create new thread
    osThreadAttr_t attr;
    memset(&attr, 0, sizeof(osThreadAttr_t));
    attr.stack_size = 4096;
    attr.name = "audio";
    th = osThreadNew(thread_audio, NULL, &attr);
    if (th == NULL) {
        MSG("Nem sikerült létrehozni a hangfeldolgozó folyamatot!\n");
    }
}

void audio_init() {
    sai_init();
    pdm2pcm_init();
    thread_init();
}

void audio_acquire() {
    HAL_StatusTypeDef status;
    if ((status = HAL_SAI_Receive_DMA(&sai, rx_buf, ACQUISITION_LENGTH * 4)) != HAL_OK) {
        MSG("%s\n", (status == HAL_BUSY) ? "BUSY" : "ERROR");
    }
}

void audio_print_output() {
    for (uint16_t i = 0; i < 1024; i++) {
        MSG("%d,", output[i]);
    }

    MSG("\n\n");
}