// --------------------------------------------------------------
#include <string.h>
#include <math.h>
#include "radio_types.h"
#include "real.h"
#include "cpx.h"

// --------------------------------------------------------------
// Basic Functions
// --------------------------------------------------------------

cpx_t CpxConjS(cpx_t v1)
{
	register cpx_t temp;

	temp.real = v1.real;
	temp.imag = -v1.imag;

	return temp;

}

cpx_t CpxAddS(cpx_t v1, cpx_t v2)
{
	register cpx_t temp;

	temp.real = v1.real + v2.real;
	temp.imag = v1.imag + v2.imag;

	return temp;
}

cpx_t CpxSubS(cpx_t v1, cpx_t v2)
{
	register cpx_t temp;

	temp.real = v1.real - v2.real;
	temp.imag = v1.imag - v2.imag;

	return temp;
}

cpx_t CpxMulS(cpx_t v1, cpx_t v2)
{
	register cpx_t t_0;
	register cpx_t t_1;

	t_0.real = v1.real*v2.real;
	t_1.real = v1.imag*v2.imag;
	t_0.imag = v1.real*v2.imag;
	t_1.imag = v1.imag*v2.real;

	t_0.real -= t_1.real;
	t_0.imag += t_1.imag;

	return t_0;
}

cpx_t CpxScaleRealS(cpx_t v1, radio_float_t v2)
{
	register cpx_t temp;

	temp.real = v1.real*v2;
	temp.imag = v1.imag*v2;

	return temp;
}

cpx_t CpxScaleComplexS(cpx_t x, cpx_t gain)
{
	register cpx_t temp;

	temp.real = x.real*gain.real;
	temp.imag = x.imag*gain.imag;

	return temp;
}

cpx_t CpxFromReal(radio_float_t v1)
{
	return Cpx(v1, v1);
}

cpx_t Cpx(radio_float_t real, radio_float_t imag)
{
	cpx_t res;

	res.real = real;
	res.imag = imag;

	return res;
}

radio_float_t CpxMagS(cpx_t v1)
{
	radio_float_t mag;

	mag = (radio_float_t)sqrt(v1.real*v1.real + v1.imag*v1.imag);

	return mag;
}

radio_float_t CpxPhiS(cpx_t v1)
{
	return (radio_float_t)atan2(v1.real, v1.imag);
}

cpx_t CpxMinS(cpx_t x1, cpx_t x2)
{
	return Cpx(dmin(x1.real, x2.real), dmin(x1.imag, x2.imag));
}

cpx_t CpxMaxS(cpx_t x1, cpx_t x2)
{
	return Cpx(dmax(x1.real, x2.real), dmax(x1.imag, x2.imag));
}

radio_float_t cpxPowerDB(cpx_t v1, radio_float_t preScale)
{
	return powerDB(CpxMagS(v1), preScale);
}

// --------------------------------------------------------------
// Complex Functions
// --------------------------------------------------------------
void CpxCopy(cpx_t *pSrc, cpx_t *pDst, uint32_t len)
{
	memcpy(pDst, pSrc, len*sizeof(cpx_t));
}

void CpxConj(cpx_t *pSrcDst, uint32_t len)
{
	uint32_t i;
	for(i=0; i < len; i++)
		pSrcDst[i] = CpxConjS(pSrcDst[i]);
}

void CpxAddB8(cpx_t *pV1, cpx_t *pV2, cpx_t *pRes)
{
	uint32_t i;

	radio_float_t r0[8];
	radio_float_t i0[8];

	radio_float_t vr0[8];
	radio_float_t vr1[8];
	radio_float_t vi0[8];
	radio_float_t vi1[8];

	for (i=0; i < 8; i++)
	{
		vr0[i] = pV1[i].real;
		vr1[i] = pV2[i].real;
		vi0[i] = pV1[i].imag;
		vi1[i] = pV2[i].imag;
	}

	for (i=0; i < 8; i++)
	{
		r0[i] = vr0[i] + vr1[i];
		i0[i] = vi0[i] + vi1[i];
	}

	for (i=0; i < 8; i++)
	{
		pRes[i].real = r0[i];
		pRes[i].imag = i0[i];
	}

}

void CpxAdd(cpx_t *pSrc, cpx_t *pSrcDst, uint32_t len)
{
	uint32_t i;
	
	for(i=0; i < len-8; i += 8)
	{
		CpxAddB8(&pSrc[i], &pSrcDst[i], &pSrcDst[i]);
	}

	for(i=i; i < len; i++)
	{
		pSrcDst[i].real += pSrc[i].real;
		pSrcDst[i].imag += pSrc[i].imag;
	}
}

void CpxSub(cpx_t *pSrc, cpx_t *pSrcDst, uint32_t len)
{
	uint32_t i;
	for(i=0; i < len; i++)
		pSrcDst[i] = CpxSubS(pSrc[i], pSrcDst[i]);
}

void CpxMulB8(cpx_t *pV1, cpx_t *pV2, cpx_t *pRes)
{
	uint32_t i;
	radio_float_t r0[8];
	radio_float_t r1[8];

	radio_float_t vr0[8];
	radio_float_t vr1[8];
	radio_float_t vi0[8];
	radio_float_t vi1[8];

	for (i=0; i < 8; i++)
	{
		vr0[i] = pV1[i].real;
		vr1[i] = pV2[i].real;
		vi0[i] = pV1[i].imag;
		vi1[i] = pV2[i].imag;
	}

	for (i=0; i < 8; i++)
	{
		r0[i] = vr0[i]*vr1[i];
	}
	for (i=0; i < 8; i++)
	{
		r1[i] = vi0[i]*vi1[i];
	}
	for (i=0; i < 8; i++)
	{
		pRes[i].real = r0[i] - r1[i];
	}

	for (i=0; i < 8; i++)
	{
		r0[i] = vr0[i]*vi1[i];
	}
	for (i=0; i < 8; i++)
	{
		r1[i] = vi0[i]*vr1[i];
	}
	for (i=0; i < 8; i++)
	{
		pRes[i].imag = r0[i] + r1[i];
	}

}

void CpxMulAddB8(cpx_t *pV1, cpx_t *pV2, cpx_t *pRes)
{
	uint32_t i;
	radio_float_t r0[8];
	radio_float_t r1[8];

	radio_float_t vr0[8];
	radio_float_t vr1[8];
	radio_float_t vi0[8];
	radio_float_t vi1[8];

	for (i=0; i < 8; i++)
	{
		vr0[i] = pV1[i].real;
		vr1[i] = pV2[i].real;
		vi0[i] = pV1[i].imag;
		vi1[i] = pV2[i].imag;
	}

	for (i=0; i < 8; i++)
	{
		r0[i] = vr0[i]*vr1[i];
	}
	for (i=0; i < 8; i++)
	{
		r1[i] = vi0[i]*vi1[i];
	}
	for (i=0; i < 8; i++)
	{
		r0[i] -= r1[i];
	}
	for (i=0; i < 8; i++)
	{
		pRes->real += r0[i];
	}

	for (i=0; i < 8; i++)
	{
		r0[i] = vr0[i]*vi1[i];
	}
	for (i=0; i < 8; i++)
	{
		r1[i] = vi0[i]*vr1[i];
	}
	for (i=0; i < 8; i++)
	{
		r0[i] += r1[i];
	}
	for (i=0; i < 8; i++)
	{
		pRes->imag += r0[i];
	}

}
void CpxScaleB8(cpx_t *v1, cpx_t v2, cpx_t *pRes)
{
	uint32_t i;
	radio_float_t r0[8];
	radio_float_t r1[8];

	radio_float_t vr0[8];
	radio_float_t vi0[8];

	for (i=0; i < 8; i++)
	{
		vr0[i] = v1[i].real;
		vi0[i] = v1[i].imag;
	}

	for (i=0; i < 8; i++)
	{
		r0[i] = vr0[i]*v2.real;
	}
	for (i=0; i < 8; i++)
	{
		r1[i] = vi0[i]*v2.imag;
	}
	for (i=0; i < 8; i++)
	{
		r0[i] -= r1[i];
	}
	for (i=0; i < 8; i++)
	{
		pRes[i].real = r0[i];
	}

	for (i=0; i < 8; i++)
	{
		r0[i] = vr0[i]*v2.imag;
	}
	for (i=0; i < 8; i++)
	{
		r1[i] = vi0[i]*v2.real;
	}
	for (i=0; i < 8; i++)
	{
		r0[i] += r1[i];
	}
	for (i=0; i < 8; i++)
	{
		pRes[i].imag = r0[i];
	}
}

void CpxMulRealB8(cpx_t *v1, radio_float_t *v2, cpx_t *pRes)
{
	uint32_t i;

	radio_float_t r0[8];
	radio_float_t i0[8];

	radio_float_t vr0[8];
	radio_float_t vi0[8];

	for (i=0; i < 8; i++)
	{
		vr0[i] = v1[i].real;
		vi0[i] = v1[i].imag;
	}

	for (i=0; i < 8; i++)
	{
		r0[i] = vr0[i]*v2[i];
		i0[i] = vi0[i]*v2[i];
	}
	for (i=0; i < 8; i++)
	{
		pRes[i].real = r0[i];
		pRes[i].imag = i0[i];
	}
}

void CpxMulRealAddB8(cpx_t *v1, radio_float_t *v2, cpx_t *pRes)
{
	uint32_t i;

	radio_float_t r0[8];
	radio_float_t i0[8];

	radio_float_t vr0[8];
	radio_float_t vi0[8];

	for (i=0; i < 8; i++)
	{
		vr0[i] = v1[i].real;
		vi0[i] = v1[i].imag;
	}

	for (i=0; i < 8; i++)
	{
		r0[i] = vr0[i]*v2[i];
		i0[i] = vi0[i]*v2[i];
	}
	for (i=0; i < 8; i++)
	{
		pRes->real += r0[i];
		pRes->imag += i0[i];
	}
}

void CpxAccumB8(cpx_t *pV1, cpx_t *pRes)
{
	uint32_t i;

	radio_float_t vr0[8];
	radio_float_t vi0[8];

	for (i=0; i < 8; i++)
	{
		vr0[i] = pV1[i].real;
		vi0[i] = pV1[i].imag;
	}

	for (i=0; i < 8; i++)
	{
		pRes->real += vr0[i];
		pRes->imag += vi0[i];
	}

}

cpx_t CpxMulAdd(cpx_t sum, cpx_t *pSrc1, cpx_t *pSrc2, uint32_t len)
{
	uint32_t i;

	for (i=0; i < (len-8); i += 8)
	{
		CpxMulAddB8(&pSrc1[i], &pSrc2[i], &sum);
	}

	for (i=i; i < len; i++)
	{
		sum = CpxAddS(sum, CpxMulS(pSrc1[i], pSrc2[i]));
	}

	return sum;
}

cpx_t CpxMulRealAdd(cpx_t sum, cpx_t *pSrc1, radio_float_t *pSrc2, uint32_t len)
{
	uint32_t i;

	for (i=0; i < (len-8); i += 8)
	{
		CpxMulRealAddB8(&pSrc1[i], &pSrc2[i], &sum);
	}

	for (i=i; i < len; i++)
	{
		sum = CpxAddS(sum, CpxScaleRealS(pSrc1[i], pSrc2[i]));
	}

	return sum;
}

void CpxMul(cpx_t *pSrc1, cpx_t *pSrc2, cpx_t *pDst, uint32_t len)
{

	uint32_t i;

	for (i=0; i < (len-8); i += 8)
	{
		CpxMulB8(&pSrc1[i], &pSrc2[i], &pDst[i]);
	}

	for (i=i; i < len; i++)
	{
		pDst[i] = CpxMulS(pSrc1[i], pSrc2[i]);
	}
}

void CpxScale(cpx_t *pSrc1, cpx_t src2, cpx_t *pDst, uint32_t len)
{

	uint32_t i;

	for (i=0; i < (len-8); i += 8)
	{
		CpxScaleB8(&pSrc1[i], src2, &pDst[i]);
	}

	for (i=i; i < len; i++)
	{
		pDst[i] = CpxMulS(pSrc1[i], src2);
	}
}

cpx_t csum(cpx_t *pSrc, int len)
{
	cpx_t sum = Cpx(0,0);
	
	while(len--)
	{
		sum.real += (*(pSrc)).real;
		sum.imag += (*(pSrc++)).imag;
	}

	return sum;
}

cpx_t cmean(cpx_t *pSrc, int len)
{
	cpx_t y;

	y = csum(pSrc, len);
	y.real /= len;
	y.imag /= len;

	return y;
}

// --------------------------------------------------------------
#include <xmmintrin.h>

void _cpxMulB4
	(
		radio_float_t pV1_real[_cpxMulB4_blockSize], radio_float_t pV1_imag[_cpxMulB4_blockSize],
		radio_float_t pV2_real[_cpxMulB4_blockSize], radio_float_t pV2_imag[_cpxMulB4_blockSize],
		radio_float_t pRes_real[_cpxMulB4_blockSize], radio_float_t pRes_imag[_cpxMulB4_blockSize])
{
	uint32_t i;
	radio_float_t r0[_cpxMulB4_blockSize];
	radio_float_t r1[_cpxMulB4_blockSize];
	radio_float_t r2[_cpxMulB4_blockSize];
	radio_float_t r3[_cpxMulB4_blockSize];

	for (i=0; i < _cpxMulB4_blockSize; i++)
	{
		r0[i] = pV1_real[i]*pV2_real[i];
	}
	for (i=0; i < _cpxMulB4_blockSize; i++)
	{
		r1[i] = pV1_imag[i]*pV2_imag[i];
	}
	for (i=0; i < _cpxMulB4_blockSize; i++)
	{
		r2[i] = pV1_real[i]*pV2_imag[i];
	}
	for (i=0; i < _cpxMulB4_blockSize; i++)
	{
		r3[i] = pV1_imag[i]*pV2_real[i];
	}
	for (i=0; i < _cpxMulB4_blockSize; i++)
	{
		pRes_real[i] = r0[i] - r1[i];
	}
	for (i=0; i < _cpxMulB4_blockSize; i++)
	{
		pRes_imag[i] = r2[i] + r3[i];
	}
}

void _cpxMulB1
	(
		radio_float_t *pV1_real, radio_float_t *pV1_imag,
		radio_float_t *pV2_real, radio_float_t *pV2_imag,
		radio_float_t *pRes_real, radio_float_t *pRes_imag)
{

	uint32_t i;
	radio_float_t r0[_cpxMulB1_blockSize];
	radio_float_t r1[_cpxMulB1_blockSize];
	radio_float_t r2[_cpxMulB1_blockSize];
	radio_float_t r3[_cpxMulB1_blockSize];

	for (i=0; i < _cpxMulB1_blockSize; i++)
	{
		r0[i] = pV1_real[i]*pV2_real[i];
	}
	for (i=0; i < _cpxMulB1_blockSize; i++)
	{
		r1[i] = pV1_imag[i]*pV2_imag[i];
	}
	for (i=0; i < _cpxMulB1_blockSize; i++)
	{
		r2[i] = pV1_real[i]*pV2_imag[i];
	}
	for (i=0; i < _cpxMulB1_blockSize; i++)
	{
		r3[i] = pV1_imag[i]*pV2_real[i];
	}
	for (i=0; i < _cpxMulB1_blockSize; i++)
	{
		pRes_real[i] = r0[i] - r1[i];
	}
	for (i=0; i < _cpxMulB1_blockSize; i++)
	{
		pRes_imag[i] = r2[i] + r3[i];
	}
}