#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "podule_api.h"
#include "ad1848.h"
#include "lark.h"

static int wss_vols[64];

static void generate_fir(ad1848_t *ad1848, int sample_rate, int cutoff);
static void ad1848_process_in_buffer(ad1848_t *ad1848, int16_t *buffer, int samples);

/*Output uses a low-pass filter with cutoff of Fs * 0.45*/
static int16_t output_iir(ad1848_t *ad1848, int i, int16_t NewSample)
{
	static double y[2][3]; //output samples
	static double x[2][3]; //input samples

	//shift the old samples
	x[i][2] = x[i][1];
	y[i][2] = y[i][1];
	x[i][1] = x[i][0];
	y[i][1] = y[i][0];

	//Calculate the new output
	x[i][0] = (double)NewSample;
	y[i][0] = ad1848->output_iir_coef_a[0] * x[i][0];
	y[i][0] += ad1848->output_iir_coef_a[1] * x[i][1] - ad1848->output_iir_coef_b[1] * y[i][1];
	y[i][0] += ad1848->output_iir_coef_a[2] * x[i][2] - ad1848->output_iir_coef_b[2] * y[i][2];

	return (int16_t)y[i][0];
}

static void output_iir_gen_coefficients(const int samplerate, const double cutoff, double* const ax, double* const by)
{
	double ff = cutoff/samplerate;
	const double ita = 1.0 / tan(M_PI*ff);
	const double q = sqrt(2.0);

	ax[0] = 1.0 / (1.0 + q*ita + ita*ita);
	ax[1]= 2*ax[0];
	ax[2]= ax[0];
	by[0] = 1.0;
	by[1] = -(2.0 * (ita*ita - 1.0) * ax[0]);
	by[2] = (1.0 - q*ita + ita*ita) * ax[0];
}

/*Input uses a brickwall low-pass FIR filter with cutoff of Fs * 0.5*/
static inline float input_fir(ad1848_t *ad1848, int i, float NewSample)
{
	static float x[2][128]; //input samples
	static int pos = 0;
	float out = 0.0;
	int read_pos;
	int n_coef;

	x[i][pos] = NewSample;

	/*Since only 1/16th of input samples are non-zero, only filter those that
	  are valid.*/
	read_pos = (pos + 15) & (127 & ~15);
	n_coef = (16 - pos) & 15;

	while (n_coef < AD1848_NCoef)
	{
		out += ad1848->low_fir_coef[n_coef] * x[i][read_pos];
		read_pos = (read_pos + 16) & (127 & ~15);
		n_coef += 16;
	}

	if (i == 1)
	{
		pos = (pos + 1) & 127;
		if (pos > 127)
			pos = 0;
	}

	return out;
}

static inline double sinc(double x)
{
	return sin(M_PI * x) / (M_PI * x);
}

static void input_fir_gen_coefficients(ad1848_t *ad1848, int sample_rate, int cutoff)
{
	/*Cutoff frequency = 1 / 32*/
	float fC = (float)cutoff / (float)sample_rate; //1.0 / 32.0;
	float gain;
	int n;

	lark_log("generate_fir: sample_rate=%i cutoff=%i fC=%f\n", sample_rate, cutoff, fC);
	for (n = 0; n < AD1848_NCoef; n++)
	{
		/*Blackman window*/
		double w = 0.42 - (0.5 * cos((2.0*n*M_PI)/(double)(AD1848_NCoef-1))) + (0.08 * cos((4.0*n*M_PI)/(double)(AD1848_NCoef-1)));
		/*Sinc filter*/
		double h = sinc(2.0 * fC * ((double)n - ((double)(AD1848_NCoef-1) / 2.0)));

		/*Create windowed-sinc filter*/
		ad1848->low_fir_coef[n] = w * h;
	}

	ad1848->low_fir_coef[(AD1848_NCoef - 1) / 2] = 1.0;

	gain = 0.0;
	for (n = 0; n < AD1848_NCoef; n++)
		gain += ad1848->low_fir_coef[n] / (float)16;

	gain /= 0.95;

	/*Normalise filter, to produce unity gain*/
	for (n = 0; n < AD1848_NCoef; n++)
		ad1848->low_fir_coef[n] /= gain;
}


uint8_t ad1848_read(ad1848_t *ad1848, uint16_t addr)
{
	uint8_t temp = 0xff;
//        lark_log("wss_read - addr %04X ", addr);
	switch (addr & 7)
	{
		case 0: case 1: case 2: case 3: /*Version*/
		temp = 4 | (ad1848->config & 0x40);
		break;

		case 4: /*Index*/
		temp = ad1848->index | ad1848->trd | ad1848->mce;
		break;
		case 5:
		temp = ad1848->regs[ad1848->index];
		break;
		case 6:
		temp = ad1848->status;
		break;
	}
//        lark_log("return %02X\n", temp);
	return temp;
}

void ad1848_write(ad1848_t *ad1848, uint16_t addr, uint8_t val)
{
	double freq, base_freq;
//        lark_log("wss_write - addr %04X val %02X\n", addr, val);
	switch (addr & 7)
	{
		case 0: case 1: case 2: case 3: /*Config*/
		ad1848->config = val;
		break;

		case 4: /*Index*/
		ad1848->index = val & 0xf;
		ad1848->trd   = val & 0x20;
		ad1848->mce   = val & 0x40;
		break;
		case 5:
		switch (ad1848->index)
		{
			case 8:
			base_freq = (val & 1) ? 16934400 : 24576000;
			switch ((val >> 1) & 7)
			{
				case 0: freq = base_freq / 3072; ad1848->rate_divider = 48; break;
				case 1: freq = base_freq / 1536; ad1848->rate_divider = 24; break;
				case 2: freq = base_freq / 896;  ad1848->rate_divider = 14; break;
				case 3: freq = base_freq / 768;  ad1848->rate_divider = 12; break;
				case 4: freq = base_freq / 448;  ad1848->rate_divider = 7;  break;
				case 5: freq = base_freq / 384;  ad1848->rate_divider = 6;  break;
				case 6: freq = base_freq / 512;  ad1848->rate_divider = 8;  break;
				case 7: freq = base_freq / 2560; ad1848->rate_divider = 40; break;
			}
			lark_log("wss freq now %f Hz\n", freq);
			input_fir_gen_coefficients(ad1848, 48000*16, freq/2);
			ad1848->in_i_inc = (int)((0x80000ull * 48000) / freq);
//                        lark_log("i_inc=%08x\n", ad1848->in_i_inc);

			ad1848->sample_freq = freq;

			ad1848->sample_time_us = 1000000.0 / freq;
			ad1848->samp_inc = ((int)(((double)(base_freq / 64) / 48000.0) * 16384.0));

			if ((val & 0xf) != (ad1848->regs[8] & 0xf))
				output_iir_gen_coefficients(base_freq / 64, (double)freq * 0.45, ad1848->output_iir_coef_a, ad1848->output_iir_coef_b);
			break;

			case 9:
			ad1848->playback_enable = ((val & 0x41) == 0x01);
			if (((val & 0x82) == 0x02) && !ad1848->capture_enable)
			{
				ad1848->in_rp = 0;
				ad1848->in_wp = 0;
				ad1848->in_ip = 0;
				ad1848->in_f_wp = 0;
				lark_sound_in_start(ad1848->lark);
			}
			if (((val & 0x82) != 0x02) && ad1848->capture_enable)
				lark_sound_in_stop(ad1848->lark);
			ad1848->capture_enable = ((val & 0x82) == 0x02);
			break;

			case 12:
			return;

			case 14:
			ad1848->count = ad1848->regs[15] | (val << 8);
//                        lark_log("wss count now %04X\n", ad1848->count);
			break;
		}
		ad1848->regs[ad1848->index] = val;
		break;
		case 6:
		ad1848->status &= 0xfe;
		lark_clear_irq(ad1848->lark, IRQ_AD1848);
		break;
	}
}

static void filter_and_downsample(ad1848_t *ad1848)
{
	int sound_write_ptr;

	if (ad1848->first_poll)
		return;

//        rpclog("mixsound: samp_fp=%i samp_wp=%i samp_rp=%i %08x %08x\n", samp_fp, samp_wp, (samp_rp >> 15) * 2, samp_rp, SAMP_INC);
	/*Apply output low-pass filter*/
	while (ad1848->fp != ad1848->wp)
	{
		ad1848->sound_in_buffer[ad1848->fp] = output_iir(ad1848, 0, ad1848->sound_in_buffer[ad1848->fp]);
		ad1848->fp++;
		ad1848->sound_in_buffer[ad1848->fp] = output_iir(ad1848, 1, ad1848->sound_in_buffer[ad1848->fp]);
		ad1848->fp++;
		ad1848->fp &= 0x7fffe;
	}

	/*Downsample to 48 kHz*/
	for (sound_write_ptr = 0; sound_write_ptr < 4800; sound_write_ptr++)
	{
		ad1848->sound_out_buffer[sound_write_ptr*2]     = ad1848->sound_in_buffer[(ad1848->rp >> 14) * 2];
		ad1848->sound_out_buffer[sound_write_ptr*2 + 1] = ad1848->sound_in_buffer[(ad1848->rp >> 14) * 2 + 1];
		ad1848->rp += ad1848->samp_inc;
	}

}

void ad1848_run(ad1848_t *ad1848, int timeslice_us)
{
	if (ad1848->in_buffer_pending_samples)
	{
		ad1848_process_in_buffer(ad1848, ad1848->in_buffer_raw, ad1848->in_buffer_pending_samples);
		ad1848->in_buffer_pending_samples = 0;
	}

	if (ad1848->playback_enable || ad1848->capture_enable)
	{
		int vol_l = (ad1848->regs[6] & 0x80) ? 0 : wss_vols[ad1848->regs[6] & 0x3f];
		int vol_r = (ad1848->regs[7] & 0x80) ? 0 : wss_vols[ad1848->regs[7] & 0x3f];

		ad1848->timing_error_us += timeslice_us;

		/*lark_log("ad1848_run: timing_error_us=%g sample_time_us=%g timeslice_us=%i\n",
			ad1848->timing_error_us,
			ad1848->sample_time_us,
			timeslice_us);*/

		while (ad1848->timing_error_us > 0)
		{
			if (ad1848->playback_enable)
			{
				int16_t samp_l, samp_r;
				int c;

				//lark_log("ad1848_run: read sample\n");
				samp_l = ad1848->dma_read(ad1848->lark);
				samp_l |= ad1848->dma_read(ad1848->lark) << 8;
				samp_r = ad1848->dma_read(ad1848->lark);
				samp_r |= ad1848->dma_read(ad1848->lark) << 8;

				samp_l = (samp_l * vol_l) >> 16;
				samp_r = (samp_r * vol_r) >> 16;

				for (c = 0; c < ad1848->rate_divider; c++)
				{
					ad1848->sound_in_buffer[ad1848->wp++] = samp_l;
					ad1848->sound_in_buffer[ad1848->wp++] = samp_r;
					ad1848->wp &= 0x7fffe;
				}
			}
			if (ad1848->capture_enable)
			{
				int16_t samp_l, samp_r, samp;

				if (ad1848->in_rp != ad1848->in_wp)
				{
					samp_l = ad1848->in_buffer[ad1848->in_rp & 0x7fff];
					ad1848->in_rp++;
					samp_r = ad1848->in_buffer[ad1848->in_rp & 0x7fff];
					ad1848->in_rp++;
				}
				else
					samp_l = samp_r = 0;

				switch (ad1848->regs[8] & 0x50)
				{
					case 0x00:
					samp = (samp_l + samp_r) / 2;
					ad1848->dma_write(ad1848->lark, (samp_l >> 8) ^ 0x80);
					ad1848->dma_write(ad1848->lark, (samp_r >> 8) ^ 0x80);
					break;
					case 0x10:
					ad1848->dma_write(ad1848->lark, (samp_l >> 8) ^ 0x80);
					ad1848->dma_write(ad1848->lark, (samp_r >> 8) ^ 0x80);
					break;
					case 0x40:
					samp = (samp_l + samp_r) / 2;
					ad1848->dma_write(ad1848->lark, samp & 0xff);
					ad1848->dma_write(ad1848->lark, samp >> 8);
					break;
					case 0x50:
					ad1848->dma_write(ad1848->lark, samp_l & 0xff);
					ad1848->dma_write(ad1848->lark, samp_l >> 8);
					ad1848->dma_write(ad1848->lark, samp_r & 0xff);
					ad1848->dma_write(ad1848->lark, samp_r >> 8);
					break;
				}
			}

			ad1848->timing_error_us -= ad1848->sample_time_us;
		}

		if (ad1848->playback_enable)
		{
			ad1848->sound_time -= timeslice_us;
			if (ad1848->sound_time < 0)
			{
				ad1848->sound_time += 100*1000; /*100ms*/

				filter_and_downsample(ad1848);
				if (ad1848->first_poll)
					ad1848->first_poll = 0;

				lark_sound_out_buffer(ad1848->lark, ad1848->sound_out_buffer, 4800);
			}
		}
	}
	else
		ad1848->timing_error_us = 0;
}

void ad1848_init(ad1848_t *ad1848, struct lark_t *lark, uint8_t (*dma_read)(struct lark_t *lark), void (*dma_write)(struct lark_t *lark, uint8_t val))
{
	int c;
	double attenuation;

//        lark_log("wss_init\n");

	ad1848->lark = lark;

	ad1848->status = 0xcc;
	ad1848->index = ad1848->trd = 0;
	ad1848->mce = 0x40;

	ad1848->regs[0] = ad1848->regs[1] = 0;
	ad1848->regs[2] = ad1848->regs[3] = 0x80;
	ad1848->regs[4] = ad1848->regs[5] = 0x80;
	ad1848->regs[6] = ad1848->regs[7] = 0x80;
	ad1848->regs[8] = 0;
	ad1848->regs[9] = 0x08;
	ad1848->regs[10] = ad1848->regs[11] = 0;
	ad1848->regs[12] = 0xa;
	ad1848->regs[13] = 0;
	ad1848->regs[14] = ad1848->regs[15] = 0;

	ad1848->first_poll = 1;

	for (c = 0; c < 64; c++)
	{
		attenuation = 0.0;
		if (c & 0x01) attenuation -= 1.5;
		if (c & 0x02) attenuation -= 3.0;
		if (c & 0x04) attenuation -= 6.0;
		if (c & 0x08) attenuation -= 12.0;
		if (c & 0x10) attenuation -= 24.0;
		if (c & 0x20) attenuation -= 48.0;

		attenuation = pow(10, attenuation / 10);

		wss_vols[c] = (int)(attenuation * 65536);
//                lark_log("wss_vols %i = %f %i\n", c, attenuation, wss_vols[c]);
	}

	ad1848->dma_read = dma_read;
	ad1848->dma_write = dma_write;

	output_iir_gen_coefficients(264600, 20000.0, ad1848->output_iir_coef_a, ad1848->output_iir_coef_b);
}

void ad1848_in_buffer(ad1848_t *ad1848, int16_t *buffer, int samples)
{
	if (!ad1848->in_buffer_pending_samples)
	{
		if (samples > 4800)
			samples = 4800;
		memcpy(ad1848->in_buffer_raw, buffer, samples*2*2);
//                lark_log("in_buffer %i\n", samples);
		ad1848->in_buffer_pending_samples = samples;
	}
}

static void ad1848_process_in_buffer(ad1848_t *ad1848, int16_t *buffer, int samples)
{
	int samples_avail = (8192*2*2) - (ad1848->in_wp - ad1848->in_rp);
	int samples_to_process;
	int prev_in_f_wp = ad1848->in_f_wp;
	int32_t in_ip_target;
	int c, d = 0, e;
	uint32_t last_read = 0;

//        lark_log("ad1848_in_buffer: %i samples, %i available\n", samples, samples_avail);
//        lark_log("  f_wp=%05x\n", ad1848->in_f_wp);

	d = ad1848->in_f_wp & 0x3ffe0;
	for (c = 0; c < samples; c++)
	{
		ad1848->filter_buffer[d++] = buffer[c*2];
		ad1848->filter_buffer[d++] = buffer[c*2 + 1];
		for (e = 1; e < 16; e++)
		{
			ad1848->filter_buffer[d++] = 0;
			ad1848->filter_buffer[d++] = 0;
		}
		d &= 0x3ffe0;
	}

	d = ad1848->in_f_wp & 0x3ffe0;
	for (c = 0; c < samples; c++)
	{
		for (e = 0; e < 16; e++)
		{
			ad1848->filter_buffer[d] = input_fir(ad1848, 0, ad1848->filter_buffer[d]);
			d++;
			ad1848->filter_buffer[d] = input_fir(ad1848, 1, ad1848->filter_buffer[d]);
			d++;
		}
		d &= 0x3ffe0;
	}
	ad1848->in_f_wp = d;
//        lark_log("  f_wp now=%05x %05x\n", ad1848->in_f_wp, samples*32);

	if (samples*2 > samples_avail)
		samples_to_process = samples_avail/2;
	else
		samples_to_process = samples;

	in_ip_target = ((ad1848->in_f_wp - prev_in_f_wp) & 0x3ffe0) << 13;
//        lark_log("  Processing %i samples\n", samples_to_process);
//        lark_log("  in_wp=%04x  in_ip=%08x %05x %05x in_ip_target=%08x\n", ad1848->in_wp, ad1848->in_ip, ad1848->in_ip >> 15, (ad1848->in_ip >> 15) * 2, in_ip_target);
	for (c = 0; c < samples_to_process; c++)
	{
		in_ip_target -= (ad1848->in_i_inc >> 1);
		if (in_ip_target <= 0)
			break;
		last_read = ad1848->in_ip;
		ad1848->in_buffer[ad1848->in_wp & 0x7fff] = ad1848->filter_buffer[(ad1848->in_ip >> 15) * 2];
		ad1848->in_wp++;
		ad1848->in_buffer[ad1848->in_wp & 0x7fff] = ad1848->filter_buffer[(ad1848->in_ip >> 15) * 2 + 1];
		ad1848->in_wp++;
		ad1848->in_ip += ad1848->in_i_inc;
	}
//        lark_log("  last_read=%08x %05x  c=%i  inc=%08x\n", last_read, (last_read >> 15) * 2, c, ad1848->in_i_inc);
	while (1)
	{
		in_ip_target -= (ad1848->in_i_inc >> 1);
		if (in_ip_target <= 0)
			break;
		ad1848->in_ip += ad1848->in_i_inc;
	}
}

int wss_irq(ad1848_t *ad1848)
{
	ad1848->status |= 0x01;
	return ad1848->regs[0xa] & 2;
}
