/*
* lab_ofdm_process.c
*
*  Created on: 6 Sep. 2016
*
*  modified NOV 21, 2016
*      Author: mckelvey
*/
#include <stdlib.h>
#include <math.h>
#include "lab_ofdm_process.h"
#include "backend/arm_math.h"
#include "blocks/sources.h"
#include "blocks/sinks.h"
#include "util.h"
#include "macro.h"
#include "config.h"
#include "backend/systime/systime.h"
#include "backend/printfn/printfn.h"
#include "arm_math.h"
#include "arm_const_structs.h"
#include "blocks/gen.h"
#include "backend/asciiplot.h"

#if defined(SYSMODE_OFDM)

#define M_ONE_OVER_SQRT_2 	(0.70710678118f)

#define LAB_OFDM_AMP_SCALE 	(6)

char message[LAB_OFDM_CHAR_MESSAGE_SIZE];
char rec_message[LAB_OFDM_CHAR_MESSAGE_SIZE];
char pilot_message[LAB_OFDM_CHAR_MESSAGE_SIZE];
float ofdm_buffer[2*LAB_OFDM_BLOCKSIZE];
float ofdm_pilot_message[2*LAB_OFDM_BLOCKSIZE];
float bb_transmit_buffer_pilot[2*(LAB_OFDM_BLOCK_W_CP_SIZE)];
float bb_transmit_buffer_message[2*(LAB_OFDM_BLOCK_W_CP_SIZE)];
float bb_transmit_buffer[2*(LAB_OFDM_BB_FRAME_SIZE)];
float bb_receive_buffer[2*(LAB_OFDM_BB_FRAME_SIZE)];
float bb_receive_buffer_message[2*(LAB_OFDM_BLOCK_W_CP_SIZE)];
float bb_receive_buffer_pilot[2*(LAB_OFDM_BLOCK_W_CP_SIZE)];
float ofdm_rx_message[2*LAB_OFDM_BLOCKSIZE];
float ofdm_rx_pilot[2*LAB_OFDM_BLOCKSIZE];
float ofdm_received_message[2*LAB_OFDM_BLOCKSIZE];
float hhat_conj[2*LAB_OFDM_BLOCKSIZE];
float soft_symb[2*LAB_OFDM_BLOCKSIZE];
uint_fast32_t rng_seed;

// LP filter with cutoff frequency = fs/LAB_OFDM_UPSAMPLE_RATE/2
// In Matlab designed with command
// R=8;[B,err] = firpm(63,[0 1/R 1/R*1.6 1],[1 1 0 0]);

// LP filter with cutoff 1/(2*R)
float lp_filter[LAB_OFDM_FILTER_LENGTH] = { -2.496956319982458e-03f, 
1.061123057910895e-03f, 1.799575981839800e-03f, 2.677675432046559e-03f, 
3.246366234977765e-03f, 3.090521427050170e-03f, 1.954953083021346e-03f, 
-1.303892962678101e-04f, -2.795326496320816e-03f, -5.356144072505601e-03f, 
-6.972193079604380e-03f, -6.867088217447664e-03f, -4.610028643185672e-03f, 
-3.312566693466183e-04f, 5.175765824001333e-03f, 1.054753460776610e-02f, 
1.411890548432881e-02f, 1.436065007374038e-02f, 1.032985773994797e-02f, 
2.132543581602375e-03f, -8.919519113938091e-03f, -2.035820813689276e-02f, 
-2.895083648554942e-02f, -3.135674961111528e-02f, -2.489424533023874e-02f, 
-8.266237880079758e-03f, 1.794823774717696e-02f, 5.118254654955430e-02f, 
8.719283811339594e-02f, 1.207736510603734e-01f, 1.467272709948181e-01f, 
1.608677439150870e-01f, 1.608677439150870e-01f, 1.467272709948181e-01f, 
1.207736510603734e-01f, 8.719283811339594e-02f, 5.118254654955430e-02f, 
1.794823774717696e-02f, -8.266237880079758e-03f, -2.489424533023874e-02f, 
-3.135674961111528e-02f, -2.895083648554942e-02f, -2.035820813689276e-02f, 
-8.919519113938091e-03f, 2.132543581602375e-03f, 1.032985773994797e-02f, 
1.436065007374038e-02f, 1.411890548432881e-02f, 1.054753460776610e-02f, 
5.175765824001333e-03f, -3.312566693466183e-04f, -4.610028643185672e-03f, 
-6.867088217447664e-03f, -6.972193079604380e-03f, -5.356144072505601e-03f, 
-2.795326496320816e-03f, -1.303892962678101e-04f, 1.954953083021346e-03f, 
3.090521427050170e-03f, 3.246366234977765e-03f, 2.677675432046559e-03f, 
1.799575981839800e-03f, 1.061123057910895e-03f, -2.496956319982458e-03f};

/* Data structures for OFDM processing */
arm_fir_decimate_instance_f32 S_decim;
float pState_decim[LAB_OFDM_TX_FRAME_SIZE+(LAB_OFDM_FILTER_LENGTH)-1];
arm_fir_interpolate_instance_f32 S_intp;
float pState_intp[(LAB_OFDM_BB_FRAME_SIZE)+(LAB_OFDM_FILTER_LENGTH/LAB_OFDM_UPSAMPLE_RATE)-1];

/* Scratch buffers for temporary storage*/
float br_tx[LAB_OFDM_TX_FRAME_SIZE], bi_tx[LAB_OFDM_TX_FRAME_SIZE];
float br_bb[LAB_OFDM_BB_FRAME_SIZE], bi_bb[LAB_OFDM_BB_FRAME_SIZE];
float pTmp[2*LAB_OFDM_BLOCKSIZE];

/** @brief Replaces non-prinable characters in a string with a '_' character. */
static void clean_str(char * inpmsg, char * outmsg, size_t outmsg_len){
	size_t i;
	for(i = 0; i < outmsg_len - 1; i++){
		if((inpmsg[i] < ' ') || (inpmsg[i] > '~')){
			outmsg[i] = '_';
		}else{
			outmsg[i] = inpmsg[i];
		}
	}
	outmsg[outmsg_len - 1] = '\0';
}
	

void lab_ofdm_process_init(void){
	arm_fir_decimate_init_f32 (&S_decim, LAB_OFDM_FILTER_LENGTH, LAB_OFDM_UPSAMPLE_RATE, lp_filter, pState_decim, LAB_OFDM_TX_FRAME_SIZE);
	arm_fir_interpolate_init_f32 (&S_intp, LAB_OFDM_UPSAMPLE_RATE, LAB_OFDM_FILTER_LENGTH, lp_filter, pState_intp, LAB_OFDM_BB_FRAME_SIZE);
	rng_seed = util_get_seed();
	lab_ofdm_process_set_randpilot(true);
	printf("OFDM initialized!\n");
}

void lab_ofdm_process_qpsk_encode(char * pMessage, float * pDst, int Mlen){
	/*
	* Encode the character string in pMessage[] of length Mlen
	* as a complex signal pDst[] with
	* QPSK encoding
	* Note that Each character requires 8 bits = 4 QPSK symbols = 4 complex
	* numbers = 8 places in the pDest array
	* Hence pDest must have a length of at least 8 * Mlen
	* Even bit numbers correspond to real part odd bit numbers correspond to imginary part
	*/
	int i, ii=0, idx=0;
	char x;
	for (i=0; i<Mlen; i++){
		x = pMessage[i];
		for (ii=0; ii<8; ii++){
			if (x & 0x01){ // test the LSB value in character
				pDst[idx++] = M_ONE_OVER_SQRT_2;
			} else {
				pDst[idx++] = -M_ONE_OVER_SQRT_2;
			}
			x = x >> 1; // shift right to make next bit LSB
		}
	}
}
void lab_ofdm_process_qpsk_decode(float * pSrc, char * pMessage,  int Mlen){
	/*
	* Decode QPSK signal pSrc[] into a character string pMessage[] of length Mlen
	*/
	int i, ii=0, idx=0;
	char x;
	for (i=0; i<Mlen; i++){
		x = 0;
		for (ii=0; ii<8; ii++){
			x = x >> 1;             // shift right 1 step
			if (pSrc[idx++] > 0) {  // Set MSB bit if signal value positive
				x |=  0x80;
			}
		}
		pMessage[i]= x;
	}
}

void add_cyclic_prefix(float * pSrc, float * pDst, int length, int cp_length){
	/* Add a cyclic prefix of length cp_length to complex signal in pSrc[]
	* with length length and cp_size refers to number of complex samples
	* and place result in pDst[]. Note that pDst[] must have a size of
	* at least length+cp_length
	*/
	int i,idx=0;
	int ii = length*2-cp_length*2;
	for (i=0; i<2*cp_length; i++){
		pDst[idx++] = pSrc[ii++];
	}
	for (i=0; i<2*length; i++) {
		pDst[idx++] = pSrc[i];
	}
}

void remove_cyclic_prefix(float *pSrc, float * pDst, int slen, int cp_size){
	/* Remove a cyclic prefix to complex signal in pSrc
	* slen and cp_size refers to number of complex samples of signal w/o prefix
	* and prefix part respectively
	*/
	int i;
	int ii = cp_size*2;
	for (i=0; i<2*slen; i++) {
		pDst[i] = pSrc[ii++];
	}
}

void ofdm_modulate(float * pRe, float * pIm, float* pDst, float f , int length){
	/*
	* Modulates a discrete time signal with the complex exponential exp(i*2*pi*f)
	* and saves the real part of the signal in vector pDst.
	* In matlab syntax;
	* pDst = real( (pRe + 1j * pIm) .* exp(1j*2*pi*f*[1:length]))
	* 
	* It is very easy (try doing this!) to show that this is equivalent to
	*
	* omega = 2*pi*f*[1:length]
	* pDst = pRe .* cos(omega) - pIm .* sin(omega)
	*/
	int i;
	float inc,omega=0;
	inc = 2*f*M_PI;
	for(i=0; i< length; i++ ){
		pDst[i] = pRe[i] * arm_cos_f32(omega) - pIm[i] * arm_sin_f32(omega);
		omega += inc;
	}
}

void cnvt_cmplx_2_re_im( float * pCmplx, float * pRe, float * pIm, int length ){
	/*
	* Convert a complex signal (pCmplx) into two vectors. One vector with the real part (pRe) and
	* one vector with the complex part (pIm). The length of the signal is length.
	*/
	int i;
	for ( i = 0; i < length ;i++) {
		pRe[i] = pCmplx[2*i];
		pIm[i] = pCmplx[2*i+1];
	}
}

void ofdm_demodulate(float * pSrc, float * pRe, float * pIm,  float f, int length ){
	/*
	* Demodulate a real signal (pSrc) into a complex signal (pRe and pPim)
	* with modulation center frequency f and length 'length'.
    *
	* Note: to avoid getting a false-fail in the self-test routine from
	* successive rounding errors, be sure to implement this function using
	* something like (using matlab notation):
	*
	* omega = 0;
	* inc = - 2*pi*f;
	* for i=1:length
	*   z(i) = pSrc(i) * exp(1j*omega);
	*	omega = omega + inc;
	* end
	* pRe = real(z);
	* pIm = imag(z);
	*
	* Be sure *NOT* to do something like:
	*
	* for i=1:length
	*	omega = -2*pi*f*(i-1);
	*   z(i) = pSrc(i) * exp(1j*omega);
	* end
	* pRe = real(z);
	* pIm = imag(z);
	*
	* The reason for this is that the term 'inc' will be rounded to the nearest
	* floating-point number and be kept constant for all interations of the for
	* loop. This is in contrast to the second case where the modulation frequency
	* might effectively jitter between different values with the lowest rounding
	* error. Though the latter gives a more accurate average frequency, we are
	* fairly sensistive to frequency jitter as this is equivalent to a change in
	* phase.
	* 
	*/
	#ifdef MASTER_MODE
#include "../../secret_sauce.h"
	DO_OFDM_DEMODULATE();
#else
	/* TODO: Add code from here... */
	float omega = 0; // 
	int i       = 0; // Loop index.
	float inc = -2*f*M_PI;

	for (i = 0; i < length; i++){
		pRe[i] = pSrc[i]*arm_cos_f32(omega);
		pIm[i] = pSrc[i]*arm_sin_f32(omega);
		omega += inc;
	}
	/* ...to here */
#endif
}

void cnvt_re_im_2_cmplx( float * pRe, float * pIm, float * pCmplx, int length ){
	/*
	* Converts a complex signal represented as pRe + sqrt(-1) * pIm into an
	* interleaved real-valued vector of size 2*length
	* I.e. pCmplx = [pRe[0], pIm[0], pRe[1], pIm[1], ... , pRe[length-1], pIm[length-1]]
	*/
#ifdef MASTER_MODE
#include "../../secret_sauce.h"
		DO_OFDM_RE_IM_2_CMPLX();
#else
	/* TODO: Add code from here... */
	int i;
	for (i = 0; i < length; i++){
		pCmplx[2*i]     = pRe[i];	
		pCmplx[2*i + 1] = pIm[i];
	}
	/* ...to here */
#endif
}

void ofdm_conj_equalize(float * prxMes, float * prxPilot,
		float * ptxPilot, float * pEqualized, float * hhat_conj, int length){
	/* Generate estimate of the channel and equalize recieved message by
	* multiplying by the conjugate of the channel.
	*
	* In this function the channel is estimated by (using matlab notation)
	* 	hhat_conj = conj(conj(ptxPilot) .* prxPilot)
	* and the recieved symbols are equalized by
	* 	pEqualized = prxMes .* hhat_conj
	* as described in the project PM. 
	* 
	* INP:
	*  prxMes[] - complex vector with received data message in frequency domain (FD)
	*  prxPilot[] - complex vector with received pilot in FD
	*  ptxPilot[] - complex vector with transmitted pilot in FD
	*  length  - number of complex OFDM symbols
	* OUT:
	*  pEqualized[] - complex vector with equalized data message (Note: only phase
	*  is equalized)
	*  hhat_conj[] -  complex vector with estimated conjugated channel gain
	*/
	//Temporary storage array for general-purpose use
	float pTmp[2*length];
#ifdef MASTER_MODE
#include "../../secret_sauce.h"
	DO_OFDM_CONJ_EQUALIZE();
#else
	/* You can use a combination of the functions
	* 	arm_cmplx_conj_f32()
	* 	arm_cmplx_mult_cmplx_f32()
	* The reference page for these DSP functions can be found here: 
	* http://www.keil.com/pack/doc/CMSIS/DSP/html/index.html 
	* The array pTmp may be freely used and is long enough to store any complex
	* vector of up to length elements. */
	
	/* TODO: Add code from here...*/
	arm_cmplx_conj_f32(ptxPilot,pTmp,length);	//Store in pTmp the complex conjugate of ptxPilot
	arm_cmplx_mult_cmplx_f32(pTmp,prxPilot,pTmp,length);	//Perform: pTmp = pTmp.*prxPilot = conj(ptxPilot).*prxPilot
	arm_cmplx_conj_f32(pTmp,hhat_conj,length);		//Calculate hhat_conj = conj(pTmp)

	arm_cmplx_mult_cmplx_f32(prxMes,hhat_conj,pEqualized,length);	//Compute the equalized signal
	/* ...to here */
#endif
}

void concat(float * pSrc1, float * pSrc2, float * pDst, int length){
	/*
	* Concatenate vectors pSrc1[] and pSrc2[] and place result in
	* pDst[]. Length of pSrc1 and pSrc2 are assumed to be length and it is
	* required that pDst has length 2*length
	*/
	int i,ii=0;
	for (i=0; i<length; i++){
		pDst[ii++] = pSrc1[i];
	}
	for (i=0; i<length; i++){
		pDst[ii++] = pSrc2[i];
	}
}
void split(float * pSrc, float * pDst1, float * pDst2, int length){
	/*
	* Split vecor pSrc into two vectors pDst1 and pDst2 each with a length length
	*/
	int i,ii=0;
	for (i=0; i<length; i++){
		pDst1[i] = pSrc[ii++];
	}
	for (i=0; i<length; i++){
		pDst2[i] = pSrc[ii++];
	}
}

void ofdm_soft_symb(float * prxMes, float * hhat_conj, float * soft_symb, int length){
	/*
	* Estimate message symbols by dividing with estimated channel
	* Note that s_k / H_k = s_k * conj(H_k) / abs(H_k)
	*/
	int i;
	float pTmp[LAB_OFDM_BLOCKSIZE];
	arm_cmplx_mult_cmplx_f32( hhat_conj, prxMes, soft_symb, length);
	arm_cmplx_mag_squared_f32(hhat_conj, pTmp, length);
	for (i=0; i<length; i++){
		soft_symb[2*i]  = soft_symb[2*i]/pTmp[i];
		soft_symb[2*i+1]  = soft_symb[2*i+1]/pTmp[i];
	}
}

void lab_ofdm_process_set_randpilot(bool randpilot_enbl){
	/* Generate pilot string */
	char rawpilot[NUMEL(pilot_message)+1];
	if(randpilot_enbl){
		blocks_gen_str(rawpilot, NUMEL(rawpilot), &rng_seed);
	}else{
		size_t i;
		for(i = 0; i < NUMEL(rawpilot); i++){
			rawpilot[i] = 'A' + i;
		}
	}
	size_t i;
	for(i = 0; i < NUMEL(pilot_message); i++){
		pilot_message[i] = rawpilot[i];
	}
}

void lab_ofdm_process_tx(float * real_tx){
	/* Create one frame including an ofdm pilot and ofdm message message block */
	/* Generate message string */
	char rawmessage[NUMEL(message)+1];

	blocks_gen_str(rawmessage, NUMEL(rawmessage), &rng_seed);
	
	/* Copy generated string to non-null-padded message strings */
	size_t i;
	for(i = 0; i < NUMEL(message); i++){
		message[i] = rawmessage[i];
	}
	
	/* Encode pilot string to qpsk sybols */
	lab_ofdm_process_qpsk_encode(pilot_message, ofdm_buffer, LAB_OFDM_CHAR_MESSAGE_SIZE);
	/* perform IFFT on ofdm_buffer */
	BUILD_BUG_ON(LAB_OFDM_BLOCKSIZE != 64);
	arm_cfft_f32(&arm_cfft_sR_f32_len64, ofdm_buffer, LAB_OFDM_IFFT_FLAG, LAB_OFDM_DO_BITREVERSE);
	/* Add cyclic prefix */
	add_cyclic_prefix(ofdm_buffer, bb_transmit_buffer_pilot, LAB_OFDM_BLOCKSIZE, LAB_OFDM_CYCLIC_PREFIX_SIZE);
	/* Encode message string to qpsk sybols */
	lab_ofdm_process_qpsk_encode( message , ofdm_buffer, LAB_OFDM_CHAR_MESSAGE_SIZE);
	/* perform IFFT on ofdm_buffer */
	BUILD_BUG_ON(LAB_OFDM_BLOCKSIZE != 64);
	arm_cfft_f32(&arm_cfft_sR_f32_len64, ofdm_buffer, LAB_OFDM_IFFT_FLAG, LAB_OFDM_DO_BITREVERSE);
	/* Add cyclic prefix */
	add_cyclic_prefix(ofdm_buffer, bb_transmit_buffer_message, LAB_OFDM_BLOCKSIZE, LAB_OFDM_CYCLIC_PREFIX_SIZE);
	/* Add Pilot + Message to create a full base band frame */
	concat(bb_transmit_buffer_pilot, bb_transmit_buffer_message, bb_transmit_buffer, 2*LAB_OFDM_BLOCK_W_CP_SIZE);
	/* Split complex signal into real and imaginary parts */
	cnvt_cmplx_2_re_im(bb_transmit_buffer, br_bb, bi_bb, LAB_OFDM_BB_FRAME_SIZE);
	/* Interpolate to the audio sampling frequency */
	arm_fir_interpolate_f32 (&S_intp, br_bb , br_tx, LAB_OFDM_BB_FRAME_SIZE);
	arm_fir_interpolate_f32 (&S_intp, bi_bb , bi_tx, LAB_OFDM_BB_FRAME_SIZE);
	/* Modulate */
	ofdm_modulate(br_tx, bi_tx, real_tx, LAB_OFDM_CENTER_FREQUENCY/AUDIO_SAMPLE_RATE, LAB_OFDM_TX_FRAME_SIZE);
	/* Scale amplitude of tranmitted signal */
	arm_scale_f32(real_tx, LAB_OFDM_AMP_SCALE, real_tx, LAB_OFDM_TX_FRAME_SIZE);
	/* buffer real_tx now ready for transmission */
	char tx_message_safe[NUMEL(message)+1];
	char pilot_message_safe[NUMEL(pilot_message)+1];
	clean_str(message, tx_message_safe, NUMEL(tx_message_safe));
	clean_str(pilot_message, pilot_message_safe, NUMEL(pilot_message_safe));
	
	printf("\n\n\n--------------------------------------------------\n");
	printf("\t\tSending message!\n");
	printf("Pilot:'%s'(non-printable characters replaced by '_')\n", pilot_message_safe);
	printf("Data: '%s'\n", tx_message_safe);
	printf("--------------------------------------------------\n");
}

void lab_ofdm_process_rx(float * real_rx_buffer, bool equalization_div){
	int i;
	
	ofdm_demodulate(real_rx_buffer, br_tx, bi_tx, LAB_OFDM_CENTER_FREQUENCY/AUDIO_SAMPLE_RATE, LAB_OFDM_TX_FRAME_SIZE);
	
	/* Decimate */
	arm_fir_decimate_f32 (&S_decim, br_tx , br_bb, LAB_OFDM_TX_FRAME_SIZE);
	arm_fir_decimate_f32 (&S_decim, bi_tx , bi_bb, LAB_OFDM_TX_FRAME_SIZE);
	
	/* Convert from real and imaginary vectors to a complex vector */
	cnvt_re_im_2_cmplx(br_bb, bi_bb, bb_receive_buffer, LAB_OFDM_BB_FRAME_SIZE);
	
	/* Split and Remove Cyclic prefix */
	split(bb_receive_buffer,bb_receive_buffer_pilot, bb_receive_buffer_message, 2*LAB_OFDM_BLOCK_W_CP_SIZE);
	remove_cyclic_prefix(bb_receive_buffer_pilot, ofdm_rx_pilot, LAB_OFDM_BLOCKSIZE, LAB_OFDM_CYCLIC_PREFIX_SIZE);
	remove_cyclic_prefix(bb_receive_buffer_message, ofdm_rx_message, LAB_OFDM_BLOCKSIZE, LAB_OFDM_CYCLIC_PREFIX_SIZE);
	
	/*  Perform FFT */
	arm_cfft_f32(&arm_cfft_sR_f32_len64, ofdm_rx_pilot, LAB_OFDM_FFT_FLAG, LAB_OFDM_DO_BITREVERSE);
	arm_cfft_f32(&arm_cfft_sR_f32_len64, ofdm_rx_message, LAB_OFDM_FFT_FLAG, LAB_OFDM_DO_BITREVERSE);
	lab_ofdm_process_qpsk_encode( pilot_message , ofdm_pilot_message, LAB_OFDM_CHAR_MESSAGE_SIZE);
	ofdm_conj_equalize(ofdm_rx_message, ofdm_rx_pilot, ofdm_pilot_message, ofdm_received_message, hhat_conj, LAB_OFDM_BLOCKSIZE);
	
	/* Decode qpsk */
	lab_ofdm_process_qpsk_decode(ofdm_received_message, rec_message, LAB_OFDM_CHAR_MESSAGE_SIZE);

	float evm = NAN;
	if(equalization_div){
		evm = 0.0f;
		/* If we equalize by division, determine the EVM of the recieved signal */
		ofdm_soft_symb(ofdm_rx_message, hhat_conj, soft_symb, LAB_OFDM_BLOCKSIZE);
		
		/* Here we calulate the "correct" symbols in the message */
		lab_ofdm_process_qpsk_encode(message, ofdm_buffer, LAB_OFDM_CHAR_MESSAGE_SIZE);
		
		/* Determine EVM for the symbols */
		float tmp[2*LAB_OFDM_BLOCKSIZE];
		arm_sub_f32(soft_symb, ofdm_buffer, tmp, 2*LAB_OFDM_BLOCKSIZE);
		arm_cmplx_mag_squared_f32(tmp, tmp, LAB_OFDM_BLOCKSIZE );
		for ( i=0; i< LAB_OFDM_BLOCKSIZE; i++){
			evm += tmp[i];
		}
		evm = sqrtf(evm/LAB_OFDM_BLOCKSIZE);
	}
	
	/* Plot constellation diagram */
	float rx_re[LAB_OFDM_BLOCKSIZE];
	float rx_im[LAB_OFDM_BLOCKSIZE];
	float tx_re[] = {M_ONE_OVER_SQRT_2, M_ONE_OVER_SQRT_2, -M_ONE_OVER_SQRT_2, -M_ONE_OVER_SQRT_2};	// Manually construct transmitted constellation diagram
	float tx_im[] = {M_ONE_OVER_SQRT_2, -M_ONE_OVER_SQRT_2, M_ONE_OVER_SQRT_2, -M_ONE_OVER_SQRT_2};
	float * symb_ptr;
	if(equalization_div){
		symb_ptr = soft_symb;
	}else{
		symb_ptr = ofdm_received_message;
	}
	cnvt_cmplx_2_re_im(symb_ptr, rx_re, rx_im, NUMEL(rx_re));
	// Set up plot to be square, with extent equal to the maximum magnitude of
	// elements in rx_re and rx_im and at least containing the transmitted symbols
	float axis_extent = 1.0f;
	for(i = 0; i < NUMEL(rx_re); i++){
		float re = fabsf(rx_re[i]);
		float im = fabsf(rx_im[i]);
		axis_extent = MAX(axis_extent, MAX(re, im));
	}
	float axis[] = {-axis_extent, axis_extent, -axis_extent, axis_extent};
	#define TITLESTR_PREFIX "Received message constellation diagram, equalized by "
	#define TITLESTR_CONJ   "R*conj(H)"
	#define TITLESTR_DIV	"R/H"
	char titlestr[NUMEL(TITLESTR_PREFIX) + NUMEL(TITLESTR_CONJ)];
	snprintfn(titlestr, NUMEL(titlestr), TITLESTR_PREFIX "%s", equalization_div ? TITLESTR_DIV : TITLESTR_CONJ);

	float *xdata[] = {rx_re, tx_re};
	float *ydata[] = {rx_im, tx_im};
	size_t data_len[] = {NUMEL(rx_re), NUMEL(tx_re)};
	char markers[] = {'*', 'O'};
	char legend_rx[] = "Receieved, equalized symbols";
	char legend_tx[] = "Transmitted symbols";
	char *legend[] = {legend_rx, legend_tx};

	struct asciiplot_s plot = {
		.cols = PLOT_COLS,
		.rows = PLOT_ROWS,
		.xdata = xdata,
		.ydata = ydata,
		.data_len = data_len,
		.num_plots = 2,
		.xlabel = "re",
		.ylabel = "im",
		.title = titlestr,
		.markers = markers,
		.legend = legend,
		.axis = axis,
		.label_prec = 3,
	};
	asciiplot_draw(&plot);
	printf("\n");
	
	/* Sanitize recieved character array to only contain printable characters. */
	char rec_message_safe[NUMEL(rec_message)+1];
	char pilot_message_safe[NUMEL(pilot_message)+1];
	char tx_message_safe[NUMEL(message)+1];
	clean_str(rec_message, rec_message_safe, NUMEL(rec_message_safe));
	clean_str(pilot_message, pilot_message_safe, NUMEL(pilot_message_safe));
	clean_str(message, tx_message_safe, NUMEL(tx_message_safe));
	
	printf("Assumed pilot:  '%s'(non-printable characters replaced by '_')\n", pilot_message_safe);
	printf("Assumed message:'%s'\n", tx_message_safe);
	printf("Received:       '%s'\n", rec_message_safe);
	if(equalization_div){
		printf("QPSK symbol EVM  %f\n\n", evm);
	}else{
		printf("QPSK symbol EVM not available with phase-only equalization\n\n");
	}
}

#endif
