/*	iobuffs.c

This file is part of a program that implements a Software-Defined Radio.

Copyright (C) 2013 Warren Pratt, NR0V

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

The author can be reached by email at

warren@wpratt.com

*/

#include "comm.h"

/********************************************************************************************************
*																										*
*											Begin Slew Code												*
*																										*
********************************************************************************************************/

enum _slew
{
	BEGIN = 0,
	DELAYUP,
	UPSLEW,
	ON,
	DELAYDOWN,
	DOWNSLEW,
	ZERO,
	OFF
};

void create_slews (IOB a)
{
	int i;
	double delta, theta;
	a->slew.ustate = BEGIN;
	a->slew.dstate = BEGIN;
	a->slew.ucount = 0;
	a->slew.dcount = 0;
	a->slew.ndelup = (int)(ch[a->channel].tdelayup * ch[a->channel].in_rate);
	a->slew.ndeldown = (int)(ch[a->channel].tdelaydown * ch[a->channel].out_rate);
	a->slew.ntup = (int)(ch[a->channel].tslewup * ch[a->channel].in_rate);
	a->slew.ntdown = (int)(ch[a->channel].tslewdown * ch[a->channel].out_rate);
	a->slew.cup	  = (double *) malloc0 ((a->slew.ntup + 1) * sizeof (double));
	a->slew.cdown = (double *) malloc0 ((a->slew.ntdown + 1) * sizeof (double));

	delta = PI / (double)a->slew.ntup;
	theta = 0.0;
	for (i = 0; i <= a->slew.ntup; i++)
	{
		a->slew.cup[i] = 0.5 * (1.0 - cos (theta));
		theta += delta;
	}

	delta = PI / (double)a->slew.ntdown;
	theta = 0.0;
	for (i = 0; i <= a->slew.ntdown; i++)
	{
		a->slew.cdown[i] = 0.5 * (1 + cos (theta));
		theta += delta;
	}

	InterlockedBitTestAndReset (&a->slew.upflag, 0);
	InterlockedBitTestAndReset (&a->slew.downflag, 0);
}

void destroy_slews(IOB a)
{
	_aligned_free (a->slew.cdown);
	_aligned_free (a->slew.cup);
}

void flush_slews (IOB a)
{
	a->slew.ustate = BEGIN;
	a->slew.dstate = BEGIN;
	a->slew.ucount = 0;
	a->slew.dcount = 0;
	InterlockedBitTestAndReset (&a->slew.upflag, 0);
	InterlockedBitTestAndReset (&a->slew.downflag, 0);
}

void upslew0 (IOB a, double* pin)
{
	int i;
	double *pout;
	double I, Q;
	pout = a->r1_baseptr + 2 * a->r1_inidx;
	for (i = 0; i < a->in_size; i++)
	{
		I = pin[2 * i + 0];
		Q = pin[2 * i + 1];
		switch (a->slew.ustate)
			{
			case BEGIN:
				pout[2 * i + 0] = 0.0;
				pout[2 * i + 1] = 0.0;
				if ((I != 0.0) || (Q != 0.0))
				{
					if (a->slew.ndelup > 0)
					{
						a->slew.ustate = DELAYUP;
						a->slew.ucount = a->slew.ndelup;
					}
					else if (a->slew.ntup > 0)
					{
						a->slew.ustate = UPSLEW;
						a->slew.ucount = a->slew.ntup;
					}
					else
						a->slew.ustate = ON;
				}
				break;
			case DELAYUP:
				pout[2 * i + 0] = 0.0;
				pout[2 * i + 1] = 0.0;
				if (a->slew.ucount-- == 0)
				{
					if (a->slew.ntup > 0)
					{
						a->slew.ustate = UPSLEW;
						a->slew.ucount = a->slew.ntup;
					}
					else
						a->slew.ustate = ON;
				}
				break;
			case UPSLEW:
				pout[2 * i + 0] = I * a->slew.cup[a->slew.ntup - a->slew.ucount];
				pout[2 * i + 1] = Q * a->slew.cup[a->slew.ntup - a->slew.ucount];
				if (a->slew.ucount-- == 0)
					a->slew.ustate = ON;
				break;
			case ON:
				pout[2 * i + 0] = I;
				pout[2 * i + 1] = Q;
				if (i == a->in_size - 1)
				{
					a->slew.ustate = BEGIN;
					InterlockedBitTestAndReset (&a->slew.upflag, 0);
				}
				break;
			}
	}
}

void upslew2 (IOB a, INREAL* pIin, INREAL* pQin)
{
	int i;
	double *pout;
	double I, Q;
	pout = a->r1_baseptr + 2 * a->r1_inidx;
	for (i = 0; i < a->in_size; i++)
	{
		I = (double)pIin[i];
		Q = (double)pQin[i];
		switch (a->slew.ustate)
			{
			case BEGIN:
				pout[2 * i + 0] = 0.0;
				pout[2 * i + 1] = 0.0;
				if ((I != 0.0) || (Q != 0.0))
				{
					if (a->slew.ndelup > 0)
					{
						a->slew.ustate = DELAYUP;
						a->slew.ucount = a->slew.ndelup;
					}
					else if (a->slew.ntup > 0)
					{
						a->slew.ustate = UPSLEW;
						a->slew.ucount = a->slew.ntup;
					}
					else
						a->slew.ustate = ON;
				}
				break;
			case DELAYUP:
				pout[2 * i + 0] = 0.0;
				pout[2 * i + 1] = 0.0;
				if (a->slew.ucount-- == 0)
				{
					if (a->slew.ntup > 0)
					{
						a->slew.ustate = UPSLEW;
						a->slew.ucount = a->slew.ntup;
					}
					else
						a->slew.ustate = ON;
				}
				break;
			case UPSLEW:
				pout[2 * i + 0] = I * a->slew.cup[a->slew.ntup - a->slew.ucount];
				pout[2 * i + 1] = Q * a->slew.cup[a->slew.ntup - a->slew.ucount];
				if (a->slew.ucount-- == 0)
					a->slew.ustate = ON;
				break;
			case ON:
				pout[2 * i + 0] = I;
				pout[2 * i + 1] = Q;
				if (i == a->in_size - 1)
				{
					a->slew.ustate = BEGIN;
					InterlockedBitTestAndReset (&a->slew.upflag, 0);
				}
				break;
			}
	}
}

void downslew0 (IOB a, double* pout)
{
	int i;
	double *pin;
	double I, Q;
	pin = a->r2_baseptr + 2 * a->r2_outidx;
	for (i = 0; i < a->out_size; i++)
	{
		I = pin[2 * i + 0];
		Q = pin[2 * i + 1];
		switch (a->slew.dstate)
			{
			case BEGIN:
				pout[2 * i + 0] = I;
				pout[2 * i + 1] = Q;
				if (a->slew.ndeldown > 0)
				{
					a->slew.dstate = DELAYDOWN;
					a->slew.dcount = a->slew.ndeldown;
				}
				else if (a->slew.ntdown > 0)
				{
					a->slew.dstate = DOWNSLEW;
					a->slew.dcount = a->slew.ntdown;
				}
				else
				{
					a->slew.dstate = ZERO;
					a->slew.dcount = a->out_size;
				}
				break;
			case DELAYDOWN:
				pout[2 * i + 0] = I;
				pout[2 * i + 1] = Q;
				if (a->slew.dcount-- == 0)
				{
					if (a->slew.ntdown > 0)
					{
						a->slew.dstate = DOWNSLEW;
						a->slew.dcount = a->slew.ntdown;
					}
					else
					{
						a->slew.dstate = ZERO;
						a->slew.dcount = a->out_size;
					}
				}
				break;
			case DOWNSLEW:
				pout[2 * i + 0] = I * a->slew.cdown[a->slew.ntdown - a->slew.dcount];
				pout[2 * i + 1] = Q * a->slew.cdown[a->slew.ntdown - a->slew.dcount];
				if (a->slew.dcount-- == 0)
				{
					a->slew.dstate = ZERO;
					a->slew.dcount = a->out_size;
				}
				break;
			case ZERO:
				pout[2 * i + 0] = 0.0;
				pout[2 * i + 1] = 0.0;
				if (a->slew.dcount-- == 0)
					a->slew.dstate = OFF;
				break;
			case OFF:
				pout[2 * i + 0] = 0.0;
				pout[2 * i + 1] = 0.0;
				if (i == a->out_size - 1)
				{
					a->slew.dstate = BEGIN;
					InterlockedBitTestAndReset (&a->slew.downflag, 0);
				}
				break;
			}
	}
}

void downslew2 (IOB a, OUTREAL* pIout, OUTREAL* pQout)
{
	int i;
	double *pin;
	double I, Q;
	pin = a->r2_baseptr + 2 * a->r2_outidx;
	for (i = 0; i < a->out_size; i++)
	{
		I = pin[2 * i + 0];
		Q = pin[2 * i + 1];
		switch (a->slew.dstate)
			{
			case BEGIN:
				pIout[i] = (OUTREAL)I;
				pQout[i] = (OUTREAL)Q;
				if (a->slew.ndeldown > 0)
				{
					a->slew.dstate = DELAYDOWN;
					a->slew.dcount = a->slew.ndeldown;
				}
				else if (a->slew.ntdown > 0)
				{
					a->slew.dstate = DOWNSLEW;
					a->slew.dcount = a->slew.ntdown;
				}
				else
				{
					a->slew.dstate = ZERO;
					a->slew.dcount = a->out_size;
				}
				break;
			case DELAYDOWN:
				pIout[i] = (OUTREAL)I;
				pQout[i] = (OUTREAL)Q;
				if (a->slew.dcount-- == 0)
				{
					if (a->slew.ntdown > 0)
					{
						a->slew.dstate = DOWNSLEW;
						a->slew.dcount = a->slew.ntdown;
					}
					else
					{
						a->slew.dstate = ZERO;
						a->slew.dcount = a->out_size;
					}
				}
				break;
			case DOWNSLEW:
				pIout[i] = (OUTREAL)(I * a->slew.cdown[a->slew.ntdown - a->slew.dcount]);
				pQout[i] = (OUTREAL)(Q * a->slew.cdown[a->slew.ntdown - a->slew.dcount]);
				if (a->slew.dcount-- == 0)
				{
					a->slew.dstate = ZERO;
					a->slew.dcount = a->out_size;
				}
				break;
			case ZERO:
				pIout[i] = 0.0;
				pQout[i] = 0.0;
				if (a->slew.dcount-- == 0)
					a->slew.dstate = OFF;
				break;
			case OFF:
				pIout[i] = 0.0;
				pQout[i] = 0.0;
				if (i == a->out_size - 1)
				{
					a->slew.dstate = BEGIN;
					InterlockedBitTestAndReset (&a->slew.downflag, 0);
				}
				break;
			}
	}
}

/********************************************************************************************************
*																										*
*										  Begin Buffer Code												*
*																										*
********************************************************************************************************/

void create_iobuffs (int channel)
{
	int n;
	IOB a = (IOB) malloc0 (sizeof(iob));
	ch[channel].iob.pc = ch[channel].iob.pd = ch[channel].iob.pe = ch[channel].iob.pf = a;
	a->channel = channel;
	a->in_size = ch[channel].in_size;
	a->r1_outsize = ch[channel].dsp_insize;
	if (a->r1_outsize > a->in_size)
		a->r1_size = a->r1_outsize;
	else
		a->r1_size = a->in_size;
	a->out_size = ch[channel].out_size;
	a->r2_insize = ch[channel].dsp_outsize;
	if (a->out_size > a->r2_insize)
		a->r2_size = a->out_size;
	else
		a->r2_size = a->r2_insize;
	a->r1_active_buffsize = DSP_MULT * a->r1_size;
	a->r2_active_buffsize = DSP_MULT * a->r2_size;
	a->r1_baseptr = (double*) malloc0 (a->r1_active_buffsize * sizeof (complex));
	a->r2_baseptr = (double*) malloc0 (a->r2_active_buffsize * sizeof (complex));
	a->r1_inidx = 0;
	a->r1_outidx = 0;
	a->r1_unqueuedsamps = 0;
	a->r2_inidx = (DSP_MULT - 1) * a->r2_size;
	a->r2_outidx = 0;
	a->r2_havesamps = (DSP_MULT - 1) * a->r2_size;
	n = a->r2_havesamps / a->out_size;
	a->r2_unqueuedsamps = a->r2_havesamps - n * a->out_size;
	InitializeCriticalSectionAndSpinCount(&a->r2_ControlSection, 2500);
	a->Sem_BuffReady = CreateSemaphore(0, 0, 1000, 0);
	a->Sem_OutReady	 = CreateSemaphore(0, n, 1000, 0);
	a->bfo = ch[channel].bfo;
	create_slews (a);

	InterlockedBitTestAndReset(&a->flush_bypass, 0);
	a->Sem_Flush = CreateSemaphore(0, 0, 1, 0);
	_beginthread(flushChannel, 0, (void*)(uintptr_t)a->channel);
}

void destroy_iobuffs (int channel)
{
	IOB a = ch[channel].iob.pc;

	InterlockedBitTestAndSet(&a->flush_bypass, 0);
	ReleaseSemaphore(a->Sem_Flush, 1, 0);
	while (InterlockedAnd(&a->flush_bypass, 0xffffffff)) Sleep(1);
	CloseHandle(a->Sem_Flush);

	destroy_slews (a);
	CloseHandle (a->Sem_OutReady);
	CloseHandle (a->Sem_BuffReady);
	DeleteCriticalSection(&a->r2_ControlSection);
	_aligned_free (a->r2_baseptr);
	_aligned_free (a->r1_baseptr);
	_aligned_free (a);
}

void flush_iobuffs (int channel)
{
	int n;
	IOB a = ch[channel].iob.pf;
	memset (a->r1_baseptr, 0, a->r1_active_buffsize * sizeof (complex));
	memset (a->r2_baseptr, 0, a->r2_active_buffsize * sizeof (complex));
	a->r1_inidx = 0;
	a->r1_outidx = 0;
	a->r1_unqueuedsamps = 0;
	a->r2_inidx = (DSP_MULT - 1) * a->r2_size;
	a->r2_outidx = 0;
	a->r2_havesamps = (DSP_MULT - 1) * a->r2_size;
	while (!WaitForSingleObject (a->Sem_BuffReady, 1));
	n = a->r2_havesamps / a->out_size;
	a->r2_unqueuedsamps = a->r2_havesamps - n * a->out_size;
	CloseHandle (a->Sem_OutReady);
	a->Sem_OutReady	 = CreateSemaphore(0, n, 1000, 0);
	flush_slews (a);
}


PORT	//double, interleaved I/Q
void fexchange0 (int channel, double* in, double* out, int* error)
{
	int n;
	int doit = 0;
	IOB a;
	*error = 0;
	if (_InterlockedAnd (&ch[channel].exchange, 1))
	{
		EnterCriticalSection (&ch[channel].csEXCH);
		a = ch[channel].iob.pe;
		if (_InterlockedAnd (&a->slew.upflag, 1))
			upslew0 (a, in);
		else
			memcpy (a->r1_baseptr + 2 * a->r1_inidx, in, a->in_size * sizeof (complex));
																											// add check with *error += -1; for case when r1 is full and an overwrite occurs
		if ((a->r1_unqueuedsamps += a->in_size) >= a->r1_outsize)
		{
			n = a->r1_unqueuedsamps / a->r1_outsize;
			ReleaseSemaphore(a->Sem_BuffReady, n, 0);
			a->r1_unqueuedsamps -= n * a->r1_outsize;
		}
		if ((a->r1_inidx += a->in_size) == a->r1_active_buffsize)
			a->r1_inidx = 0;

		EnterCriticalSection (&a->r2_ControlSection);
		if (a->r2_havesamps >= a->out_size)
			doit = 1;
		if ((a->r2_havesamps -= a->out_size) < 0) a->r2_havesamps = 0;
		LeaveCriticalSection (&a->r2_ControlSection);
		if (a->bfo) WaitForSingleObject (a->Sem_OutReady, INFINITE);
		if (a->bfo || doit)
			if (_InterlockedAnd (&a->slew.downflag, 1))
			{
				downslew0 (a, out);
				if (!_InterlockedAnd (&a->slew.downflag, 1))
				{
					InterlockedBitTestAndReset (&ch[channel].exchange, 0);
					ReleaseSemaphore(a->Sem_Flush, 1, 0);
				}
			}
			else
				memcpy (out, a->r2_baseptr + 2 * a->r2_outidx, a->out_size * sizeof (complex));
		else
		{
			memset (out, 0, a->out_size * sizeof (complex));
			*error += -2;
		}
		if ((a->r2_outidx += a->out_size) == a->r2_active_buffsize)
			a->r2_outidx = 0;
		LeaveCriticalSection (&ch[channel].csEXCH);
	}
}

PORT	//separate I/Q buffers
void fexchange2 (int channel, INREAL *Iin, INREAL *Qin, OUTREAL *Iout, OUTREAL *Qout, int* error)
{
	int i, n;
	int doit = 0;
	IOB a;
	*error = 0;
	if (_InterlockedAnd (&ch[channel].exchange, 1))
	{
		EnterCriticalSection (&ch[channel].csEXCH);
		a = ch[channel].iob.pe;
		if (_InterlockedAnd (&a->slew.upflag, 1))
			upslew2 (a, Iin, Qin);
		else
			for (i = 0; i < a->in_size; i++)
			{
				(a->r1_baseptr + 2 * a->r1_inidx)[2 * i + 0] = (double)(Iin[i]);
				(a->r1_baseptr + 2 * a->r1_inidx)[2 * i + 1] = (double)(Qin[i]);
			}
																										// add check with *error += -1; for case when r1 is full and an overwrite occurs
		if ((a->r1_unqueuedsamps += a->in_size) >= a->r1_outsize)
		{
			n = a->r1_unqueuedsamps / a->r1_outsize;
			ReleaseSemaphore(a->Sem_BuffReady, n, 0);	
			a->r1_unqueuedsamps -= n * a->r1_outsize;
		}
		if ((a->r1_inidx += a->in_size) == a->r1_active_buffsize)
			a->r1_inidx = 0;

		EnterCriticalSection (&a->r2_ControlSection);
		if (a->r2_havesamps >= a->out_size)
			doit = 1;
		if ((a->r2_havesamps -= a->out_size) < 0) a->r2_havesamps = 0;
		LeaveCriticalSection (&a->r2_ControlSection);
		if (a->bfo) WaitForSingleObject (a->Sem_OutReady, INFINITE);
		if (a->bfo || doit)
		{
			if (_InterlockedAnd (&a->slew.downflag, 1))
			{
				downslew2 (a, Iout, Qout);
				if (!_InterlockedAnd (&a->slew.downflag, 1))
				{
					InterlockedBitTestAndReset (&ch[channel].exchange, 0);
					ReleaseSemaphore(a->Sem_Flush, 1, 0);
				}
			}
			else
				for (i = 0; i < a->out_size; i++)
				{
					Iout[i] = (OUTREAL)((a->r2_baseptr + 2 * a->r2_outidx)[2 * i + 0]);
					Qout[i] = (OUTREAL)((a->r2_baseptr + 2 * a->r2_outidx)[2 * i + 1]);
				}
		}
		else
		{
			memset (Iout, 0, a->out_size * sizeof (OUTREAL));
			memset (Qout, 0, a->out_size * sizeof (OUTREAL));
			*error += -2;
		}
		if ((a->r2_outidx += a->out_size) == a->r2_active_buffsize)
			a->r2_outidx = 0;
		LeaveCriticalSection (&ch[channel].csEXCH);
	}
}

void dexchange (int channel, double* in, double* out)
{
	int n;
	IOB a = ch[channel].iob.pd;
	if (!_InterlockedAnd (&ch[channel].run, 1)) _endthread();

	EnterCriticalSection (&a->r2_ControlSection);
	a->r2_havesamps += a->r2_insize;
	LeaveCriticalSection (&a->r2_ControlSection);
	memcpy (a->r2_baseptr + 2 * a->r2_inidx, in, a->r2_insize * sizeof (complex));
	if ((a->r2_inidx += a->r2_insize) == a->r2_active_buffsize)
		a->r2_inidx = 0;
	if (a->bfo && (a->r2_unqueuedsamps += a->r2_insize) >= a->out_size)
	{
		n = a->r2_unqueuedsamps / a->out_size;
		ReleaseSemaphore(a->Sem_OutReady, n, 0);	
		a->r2_unqueuedsamps -= n * a->out_size;
	}
	memcpy (out, a->r1_baseptr + 2 * a->r1_outidx, a->r1_outsize * sizeof (complex));
	if ((a->r1_outidx += a->r1_outsize) == a->r1_active_buffsize)
		a->r1_outidx = 0;
}