/*	iqc.c

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

Copyright (C) 2013, 2016 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"

void size_iqc (IQC a)
{
	int i;
	a->t =	(double *) malloc0 ((a->ints + 1) * sizeof(double));
	for (i = 0; i <= a->ints; i++)
		a->t[i] = (double)i / (double)a->ints;
	for (i = 0; i < 2; i++)
	{
		a->cm[i] = (double *) malloc0 (a->ints * 4 * sizeof(double));
		a->cc[i] = (double *) malloc0 (a->ints * 4 * sizeof(double));
		a->cs[i] = (double *) malloc0 (a->ints * 4 * sizeof(double));
	}
	a->dog.cpi = (int *) malloc0 (a->ints * sizeof (int));
	a->dog.count = 0;
	a->dog.full_ints = 0;
}

void desize_iqc (IQC a)
{
	int i;
	_aligned_free (a->dog.cpi);
	for (i = 0; i < 2; i++)
	{
		_aligned_free (a->cm[i]);
		_aligned_free (a->cc[i]);
		_aligned_free (a->cs[i]);
	}
	_aligned_free (a->t);
}

void calc_iqc (IQC a)
{
	int i;
	double delta, theta;
	a->cset = 0;
	a->count = 0;
	a->state = 0;
	a->busy = 0;
	a->ntup = (int)(a->tup * a->rate);
	a->cup = (double *) malloc0 ((a->ntup + 1) * sizeof (double));
	delta = PI / (double)a->ntup;
	theta = 0.0;
	for (i = 0; i <= a->ntup; i++)
	{
		a->cup[i] = 0.5 * (1.0 - cos (theta));
		theta += delta;
	}
	InitializeCriticalSectionAndSpinCount (&a->dog.cs, 2500);
	size_iqc (a);
}

void decalc_iqc (IQC a)
{
	desize_iqc (a);
	DeleteCriticalSection (&a->dog.cs);
	_aligned_free (a->cup);
}

IQC create_iqc (int run, int size, double* in, double* out, double rate, int ints, double tup, int spi)
{
	IQC a = (IQC) malloc0 (sizeof (iqc));
	a->run = run;
	a->size = size;
	a->in = in;
	a->out = out;
	a->rate = rate;
	a->ints = ints;
	a->tup = tup;
	a->dog.spi = spi;
	calc_iqc (a);
	return a;
}

void destroy_iqc (IQC a)
{
	decalc_iqc (a);
	_aligned_free (a);
}

void flush_iqc (IQC a)
{

}

enum _iqcstate
{
	RUN = 0,
	BEGIN,
	SWAP,
	END,
	DONE
};

void xiqc (IQC a)
{
	if (_InterlockedAnd(&a->run, 1))
	{
		int i, k, cset, mset;
		double I, Q, env, dx, ym, yc, ys, PRE0, PRE1;
		for (i = 0; i < a->size; i++)
		{
			I = a->in[2 * i + 0];
			Q = a->in[2 * i + 1];
			env = sqrt (I * I + Q * Q);
			if ((k = (int)(env * a->ints)) > a->ints - 1) k = a->ints - 1;
			dx = env - a->t[k];
			cset = a->cset;
			ym = a->cm[cset][4 * k + 0] + dx * (a->cm[cset][4 * k + 1] + dx * (a->cm[cset][4 * k + 2] + dx * a->cm[cset][4 * k + 3]));
			yc = a->cc[cset][4 * k + 0] + dx * (a->cc[cset][4 * k + 1] + dx * (a->cc[cset][4 * k + 2] + dx * a->cc[cset][4 * k + 3]));
			ys = a->cs[cset][4 * k + 0] + dx * (a->cs[cset][4 * k + 1] + dx * (a->cs[cset][4 * k + 2] + dx * a->cs[cset][4 * k + 3]));
			PRE0 = ym * (I * yc - Q * ys);
			PRE1 = ym * (I * ys + Q * yc);

			switch (a->state)
			{
			case RUN:
				if (a->dog.cpi[k] != a->dog.spi)
					if (++a->dog.cpi[k] == a->dog.spi)
						a->dog.full_ints++;
				if (a->dog.full_ints == a->ints)
				{
					EnterCriticalSection (&a->dog.cs);
					++a->dog.count;
					LeaveCriticalSection (&a->dog.cs);
					a->dog.full_ints = 0;
					memset (a->dog.cpi, 0, a->ints * sizeof (int));
				}
				break;
			case BEGIN:
				PRE0 = (1.0 - a->cup[a->count]) * I + a->cup[a->count] * PRE0;
				PRE1 = (1.0 - a->cup[a->count]) * Q + a->cup[a->count] * PRE1;
				if (a->count++ == a->ntup)
				{
					a->state = RUN;
					a->count = 0;
					InterlockedBitTestAndReset (&a->busy, 0);
				}
				break;
			case SWAP:
				mset = 1 - cset;
				ym = a->cm[mset][4 * k + 0] + dx * (a->cm[mset][4 * k + 1] + dx * (a->cm[mset][4 * k + 2] + dx * a->cm[mset][4 * k + 3]));
				yc = a->cc[mset][4 * k + 0] + dx * (a->cc[mset][4 * k + 1] + dx * (a->cc[mset][4 * k + 2] + dx * a->cc[mset][4 * k + 3]));
				ys = a->cs[mset][4 * k + 0] + dx * (a->cs[mset][4 * k + 1] + dx * (a->cs[mset][4 * k + 2] + dx * a->cs[mset][4 * k + 3]));
				PRE0 = (1.0 - a->cup[a->count]) * ym * (I * yc - Q * ys) + a->cup[a->count] * PRE0;
				PRE1 = (1.0 - a->cup[a->count]) * ym * (I * ys + Q * yc) + a->cup[a->count] * PRE1;
				if (a->count++ == a->ntup)
				{
					a->state = RUN;
					a->count = 0;
					InterlockedBitTestAndReset (&a->busy, 0);
				}
				break;
			case END:
				PRE0 = (1.0 - a->cup[a->count]) * PRE0 + a->cup[a->count] * I;
				PRE1 = (1.0 - a->cup[a->count]) * PRE1 + a->cup[a->count] * Q;
				if (a->count++ == a->ntup)
				{
					a->state = DONE;
					a->count = 0;
					InterlockedBitTestAndReset (&a->busy, 0);
				}
				break;
			case DONE:
				PRE0 = I;
				PRE1 = Q;
				break;
			}
			a->out[2 * i + 0] = PRE0;
			a->out[2 * i + 1] = PRE1;
			// print_iqc_values("iqc.txt", a->state, env, PRE0, PRE1, ym, yc, ys, 1.1);
		}
	}
	else if (a->out != a->in)
		memcpy (a->out, a->in, a->size * sizeof (complex));
}

void setBuffers_iqc (IQC a, double* in, double* out)
{
	a->in = in;
	a->out = out;
}

void setSamplerate_iqc (IQC a, int rate)
{
	decalc_iqc (a);
	a->rate = rate;
	calc_iqc (a);
}

void setSize_iqc (IQC a, int size)
{
	a->size = size;
}

/********************************************************************************************************
*																										*
*											TXA Properties												*
*																										*
********************************************************************************************************/

PORT
void GetTXAiqcValues (int channel, double* cm, double* cc, double* cs)
{
	IQC a;
	EnterCriticalSection (&ch[channel].csDSP);
	a = txa[channel].iqc.p0;
	memcpy (cm, a->cm[a->cset], a->ints * 4 * sizeof (double));
	memcpy (cc, a->cc[a->cset], a->ints * 4 * sizeof (double));
	memcpy (cs, a->cs[a->cset], a->ints * 4 * sizeof (double));
	LeaveCriticalSection (&ch[channel].csDSP);
}

PORT
void SetTXAiqcValues (int channel, double* cm, double* cc, double* cs)
{
	IQC a;
	EnterCriticalSection (&ch[channel].csDSP);
	a = txa[channel].iqc.p0;
	a->cset = 1 - a->cset;
	memcpy (a->cm[a->cset], cm, a->ints * 4 * sizeof (double));
	memcpy (a->cc[a->cset], cc, a->ints * 4 * sizeof (double));
	memcpy (a->cs[a->cset], cs, a->ints * 4 * sizeof (double));
	a->state = RUN;
	LeaveCriticalSection (&ch[channel].csDSP);
}

PORT
void SetTXAiqcSwap (int channel, double* cm, double* cc, double* cs)
{
	IQC a = txa[channel].iqc.p1;
	EnterCriticalSection (&ch[channel].csDSP);
	a->cset = 1 - a->cset;
	memcpy (a->cm[a->cset], cm, a->ints * 4 * sizeof (double));
	memcpy (a->cc[a->cset], cc, a->ints * 4 * sizeof (double));
	memcpy (a->cs[a->cset], cs, a->ints * 4 * sizeof (double));
	InterlockedBitTestAndSet (&a->busy, 0);
	a->state = SWAP;
	a->count = 0;
	LeaveCriticalSection (&ch[channel].csDSP);
	while (_InterlockedAnd (&a->busy, 1)) Sleep(1);
}

PORT
void SetTXAiqcStart (int channel, double* cm, double* cc, double* cs)
{
	IQC a = txa[channel].iqc.p1;
	EnterCriticalSection (&ch[channel].csDSP);
	a->cset = 0;
	memcpy (a->cm[a->cset], cm, a->ints * 4 * sizeof (double));
	memcpy (a->cc[a->cset], cc, a->ints * 4 * sizeof (double));
	memcpy (a->cs[a->cset], cs, a->ints * 4 * sizeof (double));
	InterlockedBitTestAndSet (&a->busy, 0);
	a->state = BEGIN;
	a->count = 0;
	LeaveCriticalSection (&ch[channel].csDSP);
	InterlockedBitTestAndSet   (&txa[channel].iqc.p1->run, 0);
	while (_InterlockedAnd (&a->busy, 1)) Sleep(1);
}

PORT
void SetTXAiqcEnd (int channel)
{
	IQC a = txa[channel].iqc.p1;
	EnterCriticalSection (&ch[channel].csDSP);
	InterlockedBitTestAndSet (&a->busy, 0);
	a->state = END;
	a->count = 0;
	LeaveCriticalSection (&ch[channel].csDSP);
	while (_InterlockedAnd (&a->busy, 1)) Sleep(1);
	InterlockedBitTestAndReset (&txa[channel].iqc.p1->run, 0);
}

void GetTXAiqcDogCount (int channel, int* count)
{
	IQC a = txa[channel].iqc.p1;
	EnterCriticalSection (&a->dog.cs);
	*count = a->dog.count;
	LeaveCriticalSection (&a->dog.cs);
}

void SetTXAiqcDogCount (int channel, int count)
{
	IQC a = txa[channel].iqc.p1;
	EnterCriticalSection (&a->dog.cs);
	a->dog.count = count;
	LeaveCriticalSection (&a->dog.cs);
}