/**
 * pdm_mics.c
 *
 * Initializes and controls the Pulse Density Modulation (PDM) hardware for reading audio
 * data from microphones.
 * 
 * Copyright (c) 2022 Bigscreen, Inc.
 */

#include <asf.h>
#include "pdm_mics.h"
#include "qmath.h"

PDM_Mic_T pdmmic = PDM_MIC_T_DEFAULT;

// *** Buffers for USB Audio ***
int16_t pdmmic_left_buffer[PCM_BUF_SIZE];
int16_t pdmmic_right_buffer[PCM_BUF_SIZE];
int16_t pdmmic_buffer_difference; // Can be queried to check if timing is okay
uint8_t  pdmmic_streaming_buffer[USB_AUDIO_NB_CHANNELS*(((NB_SAMPLES_1MS+1UL)*2UL))];
uint16_t pdmmic_audio_streaming_offset = 0; // Current place in audio buffer to pull samples from

volatile Mixing_State_T mixstate;
q16_t fractional_gain;

void init_pdm(void) {
	
	/* PDM configuration structure */
	struct pdm_config conf;
	/* Get default configuration */
	pdm_get_config_default(&conf);

	/* Prescaler for PDM clock */
	uint32_t prescaler = (CONFIG_PLL0_FREQ / PDM_CLK_FREQ / 2UL) - 1UL;
	conf.prescal = prescaler;
	/* Set gain to 1 - if not, all conversions are 0 */
	conf.gain = PDM_GAIN;
	/* Oversampling ratio */
	#if (PDM_OVERSAMPLING_RATIO == 64UL)
	conf.oversampling_ratio = PDMIC_OVERSAMPLING_RATIO_64;
	#elif (PDM_OVERSAMPLING_RATIO == 128UL)
	conf.oversampling_ratio = PDMIC_OVERSAMPLING_RATIO_128;
	#else
	#error "PDM_OVERSAMPLING_RATIO must be 64 or 128"
	#endif
	/* Data size */
	conf.conver_data_size = PDMIC_CONVERTED_DATA_SIZE_16;
	struct pdm_instance pdm0, pdm1;
	/* Initialize PDMIC0 with configuration */
	pdm_init(&pdm0, PDMIC0, &conf);
	/* Initialize PDMIC1 with configuration */
	pdm_init(&pdm1, PDMIC1, &conf);
	pdm_enable(&pdm0);
	pdm_enable(&pdm1);
	pdmmic.initialized = pdTRUE;
	
		
	// mixstate = MIX_STATE_STEREO;
	// Edited Jan 31, 2023. Changed default to MONO
	mixstate = MIX_STATE_ADD;
	fractional_gain = PDM_FRACTIONAL_GAIN;
}

void disable_pdm(void) {
	stop_pdm_transfer();
	
	struct pdm_instance pdm0, pdm1;
	pdm_disable(&pdm0);
	pdm_disable(&pdm1);
	
	sysclk_disable_peripheral_clock(ID_PDMIC0);
	sysclk_disable_peripheral_clock(ID_PDMIC1);
	pdmmic.initialized = pdFALSE;
}

void set_pdm_buffers(int16_t* leftbuf, int16_t* rightbuf, uint16_t buflen) {
	if(buflen != 0 && leftbuf != 0 && rightbuf != 0) {
		pdmmic.left_buffer = leftbuf;
		pdmmic.right_buffer = rightbuf;
		pdmmic.buffer_length = buflen;
	}
}

void start_pdm_transfer(void) {
	struct pdc_packet packet;
	Pdc *pdmic0_pdc, *pdmic1_pdc;
	if(pdmmic.initialized && pdmmic.buffer_length != 0 && pdmmic.left_buffer != 0 && pdmmic.right_buffer != 0)
	{
		pdmic0_pdc = pdmic_get_pdc_base(PDMIC0);
		pdmic1_pdc = pdmic_get_pdc_base(PDMIC1);
	
		/* Set buffer address and size */
		packet.ul_addr = (uint32_t)&(pdmmic.left_buffer[0]);
		packet.ul_size = pdmmic.buffer_length;

		/* Initialize and enable PDC */
		//pdc_rx_init(pdmic_pdc, &packet, NULL); // When not using circular mode, don't use "next" registers
		pdc_rx_init(pdmic0_pdc, &packet, &packet); // same packet for current and next for circular DMA
		pdmic0_pdc->PERIPH_PTCR = PERIPH_PTCR_RXCBEN;
		packet.ul_addr = (uint32_t)&(pdmmic.right_buffer[0]);
		pdc_rx_init(pdmic1_pdc, &packet, &packet);
		pdmic1_pdc->PERIPH_PTCR = PERIPH_PTCR_RXCBEN;
		pdc_enable_transfer(pdmic0_pdc, PERIPH_PTCR_RXTEN);
		pdc_enable_transfer(pdmic1_pdc, PERIPH_PTCR_RXTEN);
	}
}

void stop_pdm_transfer(void) {
	Pdc *pdmic0_pdc, *pdmic1_pdc;
	pdmic0_pdc = pdmic_get_pdc_base(PDMIC0);
	pdmic1_pdc = pdmic_get_pdc_base(PDMIC1);
	pdmic0_pdc->PERIPH_PTCR = PERIPH_PTCR_RXTDIS;
	pdmic1_pdc->PERIPH_PTCR = PERIPH_PTCR_RXTDIS;
}

uint16_t get_pdm_buffer_position(void) {
	if(pdmmic.initialized) {
		return (uint16_t)(((uint32_t)pdmmic.buffer_length) - PDMIC0->PDMIC_RCR);
	} else {
		return 0;
	}
}

void set_pdm_gain(uint16_t newgain) {
	uint32_t dspr1 = PDMIC0->PDMIC_DSPR1;
	dspr1 &= ~(PDMIC_DSPR1_DGAIN_Msk);
	dspr1 |= PDMIC_DSPR1_DGAIN(newgain);
	PDMIC0->PDMIC_DSPR1 = dspr1;
	
	dspr1 = PDMIC1->PDMIC_DSPR1;
	dspr1 &= ~(PDMIC_DSPR1_DGAIN_Msk);
	dspr1 |= PDMIC_DSPR1_DGAIN(newgain);
	PDMIC1->PDMIC_DSPR1 = dspr1;
}

void set_pdm_fractional_gain(int16_t newgain) {
	fractional_gain = (q16_t)newgain;
}

void pdmmic_set_mixstate(Mixing_State_T mix) {
	mixstate = mix;
}

Mixing_State_T pdmmic_get_mixstate(void) {
	return mixstate;
}

int16_t* pdmmic_get_audio_left_buffer(void) {
	return pdmmic_left_buffer;
}
int16_t* pdmmic_get_audio_right_buffer(void) {
	return pdmmic_right_buffer;
}

void pdmmic_audio_enable(void) {
	
}

void pdmmic_audio_disable(void) {
	
}

/** 
 * \brief This task copies data from the microphone buffers into the USB
 *	endpoint
 */
void task_usbload(void *pvParameters) {
	UNUSED(pvParameters);
	
	int16_t* mainbuf = pdmmic_get_audio_left_buffer();
	int16_t* secondbuf = pdmmic_get_audio_right_buffer();
	uint16_t mainbufpos;
	uint16_t buff_difference;
	q16_t qtemp;
	//xUSBLoadTaskHandle = xTaskGetCurrentTaskHandle();
	
	while(pdTRUE) {
		ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
		//taskENTER_CRITICAL();
	
		mainbufpos = get_pdm_buffer_position();
		if(mainbufpos >= pdmmic_audio_streaming_offset)  {
			buff_difference = mainbufpos - pdmmic_audio_streaming_offset;
			} else {
			buff_difference = PCM_BUF_SIZE + mainbufpos - pdmmic_audio_streaming_offset;
		}
	
		// Send one extra sample if we are too fast, one fewer if too slow
		uint16_t num_samples_to_send = NB_SAMPLES_1MS;
		if(buff_difference > NB_SAMPLES_1MS) {
			num_samples_to_send++;
			} else if(buff_difference < NB_SAMPLES_1MS) {
			num_samples_to_send--;
		}
	
		for(uint16_t i = 0; i < num_samples_to_send; i++) {
			uint16_t main_buf_place = i + pdmmic_audio_streaming_offset;
		
			if(main_buf_place >= PCM_BUF_SIZE) {
				// wraparound case
				main_buf_place -= PCM_BUF_SIZE;
			}
			if(mixstate == MIX_STATE_STEREO) {
				qtemp = qmult(fractional_gain, (q16_t)mainbuf[main_buf_place]);
				pdmmic_streaming_buffer[4*i] = (uint8_t)((int16_t)qtemp & 0x00FF);
				pdmmic_streaming_buffer[4*i+1] = (uint8_t)(((int16_t)qtemp & 0xFF00) >> 8);
				qtemp = qmult(fractional_gain, (q16_t)secondbuf[main_buf_place]);
				pdmmic_streaming_buffer[4*i+2] = (uint8_t)((int16_t)qtemp & 0x00FF);
				pdmmic_streaming_buffer[4*i+3] = (uint8_t)(((int16_t)qtemp & 0xFF00) >> 8);
				
				/*
				pdmmic_streaming_buffer[4*i] = (uint8_t)(mainbuf[main_buf_place] & 0x00FF);
				pdmmic_streaming_buffer[4*i+1] = (uint8_t)((mainbuf[main_buf_place] & 0xFF00) >> 8);
				pdmmic_streaming_buffer[4*i+2] = (uint8_t)(secondbuf[main_buf_place] & 0x00FF);
				pdmmic_streaming_buffer[4*i+3] = (uint8_t)((secondbuf[main_buf_place] & 0xFF00) >> 8);
				*/
			} else if(mixstate == MIX_STATE_ADD) {
				int32_t newsample;
				// add the two samples together and average
				newsample = (mainbuf[main_buf_place] + secondbuf[main_buf_place]) / 2;
				// clip at +32767, -32768
				if(newsample <= -32769) {
					newsample = -32768;
				} else if(newsample >= 32768) {
					newsample = 32767;
				}
				// Same sample sent to both channels
				qtemp = qmult(fractional_gain, (q16_t)newsample);
				pdmmic_streaming_buffer[4*i] = (uint8_t)(qtemp & 0x00FF);
				pdmmic_streaming_buffer[4*i+1] = (uint8_t)((qtemp & 0xFF00) >> 8);
				pdmmic_streaming_buffer[4*i+2] = (uint8_t)(qtemp & 0x00FF);
				pdmmic_streaming_buffer[4*i+3] = (uint8_t)((qtemp & 0xFF00) >> 8);
			}
		}

		pdmmic_buffer_difference = buff_difference;

		pdmmic_audio_streaming_offset += num_samples_to_send;
		if(pdmmic_audio_streaming_offset >= PCM_BUF_SIZE) {
			pdmmic_audio_streaming_offset -= PCM_BUF_SIZE;
		}
		udi_audio_send(pdmmic_streaming_buffer, num_samples_to_send*2UL*USB_AUDIO_NB_CHANNELS);
		//taskEXIT_CRITICAL();
	}
}