// --------------------------------------------------------------
#include <stdio.h>		// for SymStatPrint() remove later
#include <string.h>
#include <malloc.h>
#include <math.h>
#include "radio_types.h"
#include "symbol.h"

// --------------------------------------------------------------
// Symbol demapper for receiver works on each symbol
// --------------------------------------------------------------
void SymMapInit(sym_map_t *pObj, uint32_t nBitsPerSym, uint32_t type)
{
	cpx_t iq;
	uint32_t i, j, k, m, n, u, Ns, Nq, Nsq, map_idx_i, map_idx_q;
	radio_float_t phi, dphi, ii, qq, iii, qqq, temp, stepIQ;
	radio_float_t mag, E_m2, E_m2_c, E_m4;
	radio_float_t c_smma;
	cpx_t E_a4;
	int signI[4] = {+1, -1, -1, +1};
	int signQ[4] = {+1, +1, -1, -1};
	int even;
	pObj->nBitsPerSym = nBitsPerSym;
	pObj->num_const = 1;
	pObj->type = type;
	pObj->iq_step = 0;
	pObj->iq_max = 0;
	pObj->Pref = 0;
	E_a4 = Cpx(0,0);
	c_smma = (radio_float_t)1;			// Tuning constant S-MMA
	for(k=0; k < nBitsPerSym; k++)
	{
		pObj->num_const *= 2;
	}
	pObj->pMapTbl = (map_t*)malloc(pObj->num_const*sizeof(map_t));
	pObj->ppMapTbl = NULL;

	Ns = (uint32_t)sqrt((radio_float_t)pObj->num_const);
	Nq = pObj->num_const/4;
	Nsq = (uint32_t)sqrt((radio_float_t)Nq);
	stepIQ = (radio_float_t)sqrt(2.0)/(Ns-1);
	E_m2_c = E_m2 = E_m4 = 0;
	switch(type)
	{

		case MAP_MODE_PSK:

		pObj->Es = 1.0;						// Symbol transmit power
		pObj->Eb = pObj->Es/nBitsPerSym;
		pObj->Pref = (radio_float_t)(1.0/sqrt(2.0));

		dphi = 2*(radio_float_t)PI/pObj->num_const;
		phi = (radio_float_t)PI/pObj->num_const;
		
		for (k=0; k < pObj->num_const; k++)
		{
			iq.real = (radio_float_t)cos(phi);
			iq.imag = (radio_float_t)sin(phi);
			pObj->pMapTbl[k].phi = (radio_float_t)CpxPhiS(iq);
			pObj->pMapTbl[k].mag = (radio_float_t)CpxMagS(iq);
			pObj->pMapTbl[k].rect = iq;
			phi += dphi;
		}
		break;

		case MAP_MODE_QAM:
		pObj->Es = (radio_float_t)0.5*stepIQ*stepIQ;
		pObj->Eb = pObj->Es/nBitsPerSym;
		pObj->ppMapTbl = (map_t**)malloc(Ns*sizeof(map_t*));
		for (i=0; i< Ns; i++)
			pObj->ppMapTbl[i] = (map_t*)malloc(Ns*sizeof(map_t));

		ii = (radio_float_t)(1.0/sqrt(2.0));
		pObj->iq_max = ii;
		qq = ii;
		even = 1;

		k = 0;
		for (m=0; m < Nsq; m++)
		{
			for (n=0; n < Nsq; n++)
			{
				iii = ii;
				qqq = qq;
				for (j=0; j < 4; j++)
				{
					u = Nq*j+k;
					iq.real = iii*signI[j];
					iq.imag = qqq*signQ[j];
					mag = (radio_float_t)CpxMagS(iq);
					phi = (radio_float_t)CpxPhiS(iq);
					pObj->pMapTbl[u].mag = mag;
					pObj->pMapTbl[u].phi = phi;
					pObj->pMapTbl[u].rect = iq;
					E_m2_c += (radio_float_t)pow(mag, 2+c_smma);
					E_m2 += (radio_float_t)pow(mag, 2);
					E_m4 += (radio_float_t)pow(mag, 4);
					E_a4.real += (radio_float_t)pow(iq.real, 4);
					E_a4.imag += (radio_float_t)pow(iq.imag, 4);
					temp = iii;
					iii = qqq;
					qqq = temp;
					map_idx_i = (int)(0.5 +(0.5*iq.real/pObj->iq_max + 0.5)*(Ns-1));
					map_idx_q = (int)(0.5 +(0.5*iq.imag/pObj->iq_max + 0.5)*(Ns-1));
					pObj->ppMapTbl[map_idx_i][map_idx_q].mag = mag;
					pObj->ppMapTbl[map_idx_i][map_idx_q].phi = phi;
					pObj->ppMapTbl[map_idx_i][map_idx_q].rect = iq;
					pObj->ppMapTbl[map_idx_i][map_idx_q].sym = (symbol_t)u;
				}
				ii = ii - even*stepIQ;
				k++;
			}
			qq = qq - stepIQ;
			ii = ii + even*stepIQ;
			even = -even;
		}
		
		switch(pObj->num_const)
		{
			// QPSK
			case 4:
				pObj->Pref = (radio_float_t)0.707;
				break;
			// 16-QAM
			case 16:
				pObj->Pref = (radio_float_t)0.385;
				break;
			// 64-QAM
			case 64:
				pObj->Pref = (radio_float_t)0.31;
				break;
			// 256-QAM
			case 256:
				pObj->Pref = (radio_float_t)0.27;
				break;
			default:
			break;
		}
		break;

		default:
		break;
	}
	pObj->iq_step = stepIQ;
	pObj->iq_side_len = Ns;
	pObj->R2_cma = E_m4/E_m2;
	pObj->R_mma.real = 2*E_a4.real/E_m2;
	pObj->R_mma.imag = 2*E_a4.imag/E_m2;
	pObj->R_smma.real = 2*E_a4.real/E_m2_c;
	pObj->R_smma.imag = 2*E_a4.imag/E_m2_c;
}

void SymMapFree(sym_map_t *pObj)
{
	uint32_t i;

	if (pObj->pMapTbl)
	{
		free(pObj->pMapTbl);
		pObj->pMapTbl = NULL;
	}

	if (pObj->ppMapTbl)
	{
		for (i=0; i< (uint32_t)sqrt((float)pObj->num_const); i++)
			free(pObj->ppMapTbl[i]);

		free(pObj->ppMapTbl);
		pObj->ppMapTbl = NULL;
	}
}

void SymMapReinit(sym_map_t *pObj, uint32_t nBitsPerSym, uint32_t type)
{
	SymMapFree(pObj);
	SymMapInit(pObj, nBitsPerSym, type);
}

cpx_t SymMapMap(sym_map_t *pObj, symbol_t symbol)
{
	if (symbol >= pObj->num_const)
		return Cpx(0,0);

	return pObj->pMapTbl[(uint32_t)symbol].rect;
}

symbol_t SymMapDemap(sym_map_t *pObj, cpx_t x)
{
	uint32_t map_idx_i, map_idx_q;
	radio_float_t rx_i, rx_q;

	rx_i = dclamp(x.real, -pObj->iq_max, pObj->iq_max);
	rx_q = dclamp(x.imag, -pObj->iq_max, pObj->iq_max);

	map_idx_i = (int)(0.5 +(0.5*rx_i/pObj->iq_max + 0.5)*(pObj->iq_side_len-1));
	map_idx_q = (int)(0.5 +(0.5*rx_q/pObj->iq_max + 0.5)*(pObj->iq_side_len-1));
	
	return pObj->ppMapTbl[map_idx_i][map_idx_q].sym;
}

map_t SymMapGetSymbolInfo(sym_map_t *pObj, symbol_t symbol)
{
	return pObj->pMapTbl[(uint32_t)symbol];
}

sym_err_t SymMapGetError(sym_map_t *pObj, cpx_t x, symbol_t symbol)
{
	map_t map;
	sym_err_t res;

	map = SymMapGetSymbolInfo(pObj, symbol);

	res.err_mag = map.mag - (radio_float_t)CpxMagS(x);
	res.err_phi = map.phi - (radio_float_t)CpxPhiS(x);
	res.err = CpxSubS(map.rect, x);
	res.mag = CpxMagS(x);
	res.phi = CpxPhiS(x);
	res.soft_sym = x;
	res.hard_sym = map.rect;
	res.hard_mag = map.mag;
	res.hard_phi = map.phi;

	return res;

}

// --------------------------------------------------------------
void SymStatInit(sym_stat_t *pObj, uint32_t nBitsPerSym)
{
	uint32_t i;

	memset(pObj, 0, sizeof(sym_stat_t));

	pObj->num_const = 1;
	for(i=0; i < nBitsPerSym; i++)
	{
		pObj->num_const *= 2;
	}

	pObj->pPerSymStat = (per_sym_stat_t*)malloc(pObj->num_const*sizeof(per_sym_stat_t));

	for(i=0; i < pObj->num_const; i++)
	{
		pObj->pPerSymStat[i].count = 0;
		pObj->pPerSymStat[i].var_err_mag = 0;
		pObj->pPerSymStat[i].var_err_phi = 0;
		SlidingVarInit(&pObj->pPerSymStat[i].sl_err_mag, 1000, 1000);
		SlidingVarInit(&pObj->pPerSymStat[i].sl_err_phi, 1000, 1000);
	}

}

void SymStatFree(sym_stat_t *pObj)
{
	if(pObj->pPerSymStat)
		free(pObj->pPerSymStat);

	pObj->pPerSymStat = 0;
}

void SymStatReset(sym_stat_t *pObj)
{
	uint32_t i;
	pObj->sym_cnt = 0;
	for(i=0; i < pObj->num_const; i++)
	{
		pObj->pPerSymStat[i].count = 0;
	}
}

void SymStatUpDate(sym_stat_t *pObj, symbol_t sym, sym_err_t *pSym_err)
{
	// Statistics
	pObj->sym_cnt++;
	pObj->pPerSymStat[sym].count++;
	pObj->pPerSymStat[sym].p = (radio_float_t)pObj->pPerSymStat[sym].count/pObj->sym_cnt;
	pObj->pPerSymStat[sym].var_err_mag = SlidingVarProcess(&pObj->pPerSymStat[sym].sl_err_mag, pSym_err->err_mag);
	pObj->pPerSymStat[sym].var_err_phi = SlidingVarProcess(&pObj->pPerSymStat[sym].sl_err_phi, pSym_err->err_phi);

}

void SymStatPrint(sym_stat_t *pObj)
{
	uint32_t i;

	printf("Number of constellations : %d\n", pObj->num_const);
	printf("Number of total symbols  : %d\n", pObj->sym_cnt);
	printf("Sym#\tOccur.\t\tProb.\t\tVar(MagErr)[dB]\tVar(PhiErr/PI)[dB]\n");
	for(i=0; i < pObj->num_const; i++)
	{
		printf("%4.1d\t%6.d\t\t%1.2g\t\t%.2f\t\t%.2f\n", 
		i,
		pObj->pPerSymStat[i].count,
		pObj->pPerSymStat[i].p,
		10*log10(pObj->pPerSymStat[i].var_err_mag),
		10*log10(pObj->pPerSymStat[i].var_err_phi/(PI*PI)));
	}
}