/*
 * libiio - Edited AD9361 IIO streaming example
 * See the original at: https://github.com/analogdevicesinc/libiio/blob/master/examples/ad9361-iiostream.c
 *
 */

/*	Set these defines to prevent fopen() warning and to be able to use
	constants from math.h
*/
#define _CRT_SECURE_NO_WARNINGS
#define _USE_MATH_DEFINES

#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <signal.h>
#include <stdio.h>
#include <iio.h>
#include <math.h>

/* debugging macros */
//#define DEBUG_SINCOS
//#define USE_DITHERING
#define RX_ENABLED
#define TX_ENABLED

/* helper macros */
#define MHZ(x) ((long long)(x*1000000.0 + .5))
#define GHZ(x) ((long long)(x*1000000000.0 + .5))

#define TX_BUFFER_SIZE 1024*1024*2
#define RX_BUFFER_SIZE 1024*1024*2
#define RX_BUFFER_COUNT 4
#define TX_BUFFER_COUNT 4

#define ASSERT(expr) { \
	if (!(expr)) { \
		(void) fprintf(stderr, "assertion failed (%s:%d)\n", __FILE__, __LINE__); \
		(void) abort(); \
	} \
}

/* RX is input, TX is output */
enum iodev { RX, TX };

/* common RX and TX streaming params */
struct stream_cfg {
	long long bw_hz; // Analog banwidth in Hz
	long long fs_hz; // Baseband sample rate in Hz
	long long lo_hz; // Local oscillator frequency in Hz
	const char* rfport; // Port name
};

/* static scratch mem for strings */
static char tmpstr[64];

/* IIO structs required for streaming */
static struct iio_context *ctx   = NULL;
static struct iio_channel *rx0_i = NULL;
static struct iio_channel *rx0_q = NULL;
static struct iio_channel *tx0_i = NULL;
static struct iio_channel *tx0_q = NULL;
static struct iio_buffer  *rxbuf = NULL;
static struct iio_buffer  *txbuf = NULL;

static bool stop = false;

// Writing RX samples to a file
FILE *rx_capture = NULL;
const char *filename = "e:\\capture_rx.bin";

// Lookup tables for sine and cosine generation
int16_t *sin_table = NULL;
int16_t *cos_table = NULL;

/* cleanup and exit */
static void shutdown()
{
	printf("* Destroying sine and cosine lookup tables\n");
	free(sin_table);
	free(cos_table);

	printf("* Flushing RX capture file to disk\n");
	fflush(rx_capture);

	printf("* Closing RX capture file\n");
	fclose(rx_capture);

	printf("* Destroying buffers\n");
	if (rxbuf) { iio_buffer_destroy(rxbuf); }
	if (txbuf) { iio_buffer_destroy(txbuf); }

	printf("* Disabling streaming channels\n");
	if (rx0_i) { iio_channel_disable(rx0_i); }
	if (rx0_q) { iio_channel_disable(rx0_q); }
	if (tx0_i) { iio_channel_disable(tx0_i); }
	if (tx0_q) { iio_channel_disable(tx0_q); }

	printf("* Destroying context\n");
	if (ctx) { iio_context_destroy(ctx); }
	system("PAUSE");
	exit(0);
}

static void handle_sig(int sig)
{
	printf("Waiting for process to finish...\n");
	stop = true;
}

/* Generating noise for dithering */
long long noise(long long max_abs_value)
{
	return (((long long)rand() % (max_abs_value + 1)) - max_abs_value);
}

/* check return value of attr_write function */
static void errchk(int v, const char* what) {
	if (v < 0) { fprintf(stderr, "Error %d writing to channel \"%s\"\nvalue may not be supported.\n", v, what); shutdown(); }
}

/* write attribute: long long int */
static void wr_ch_lli(struct iio_channel *chn, const char* what, long long val)
{
	errchk(iio_channel_attr_write_longlong(chn, what, val), what);
}

/* write attribute: string */
static void wr_ch_str(struct iio_channel *chn, const char* what, const char* str)
{
	errchk(iio_channel_attr_write(chn, what, str), what);
}

/* helper function generating channel names */
static char* get_ch_name(const char* type, int id)
{
	snprintf(tmpstr, sizeof(tmpstr), "%s%d", type, id);
	return tmpstr;
}

/* returns ad9361 phy device */
static struct iio_device* get_ad9361_phy(struct iio_context *ctx)
{
	struct iio_device *dev =  iio_context_find_device(ctx, "ad9361-phy");
	ASSERT(dev && "No ad9361-phy found");
	return dev;
}

/* finds AD9361 streaming IIO devices */
static bool get_ad9361_stream_dev(struct iio_context *ctx, enum iodev d, struct iio_device **dev)
{
	switch (d) {
	case TX: *dev = iio_context_find_device(ctx, "cf-ad9361-dds-core-lpc"); return *dev != NULL;
	case RX: *dev = iio_context_find_device(ctx, "cf-ad9361-lpc");  return *dev != NULL;
	default: ASSERT(0); return false;
	}
}

/* finds AD9361 streaming IIO channels */
static bool get_ad9361_stream_ch(struct iio_context *ctx, enum iodev d, struct iio_device *dev, int chid, struct iio_channel **chn)
{
	*chn = iio_device_find_channel(dev, get_ch_name("voltage", chid), d == TX);
	if (!*chn)
		*chn = iio_device_find_channel(dev, get_ch_name("altvoltage", chid), d == TX);
	return *chn != NULL;
}

/* finds AD9361 phy IIO configuration channel with id chid */
static bool get_phy_chan(struct iio_context *ctx, enum iodev d, int chid, struct iio_channel **chn)
{
	switch (d) {
	case RX: *chn = iio_device_find_channel(get_ad9361_phy(ctx), get_ch_name("voltage", chid), false); return *chn != NULL;
	case TX: *chn = iio_device_find_channel(get_ad9361_phy(ctx), get_ch_name("voltage", chid), true);  return *chn != NULL;
	default: ASSERT(0); return false;
	}
}

/* finds AD9361 local oscillator IIO configuration channels */
static bool get_lo_chan(struct iio_context *ctx, enum iodev d, struct iio_channel **chn)
{
	switch (d) {
	 // LO chan is always output, i.e. true
	case RX: *chn = iio_device_find_channel(get_ad9361_phy(ctx), get_ch_name("altvoltage", 0), true); return *chn != NULL;
	case TX: *chn = iio_device_find_channel(get_ad9361_phy(ctx), get_ch_name("altvoltage", 1), true); return *chn != NULL;
	default: ASSERT(0); return false;
	}
}

/* applies streaming configuration through IIO */
bool cfg_ad9361_streaming_ch(struct iio_context *ctx, struct stream_cfg *cfg, enum iodev type, int chid)
{
	struct iio_channel *chn = NULL;

	// Configure phy and lo channels
	printf("* Acquiring AD9361 phy channel %d\n", chid);
	if (!get_phy_chan(ctx, type, chid, &chn)) {	return false; }
	wr_ch_str(chn, "rf_port_select",     cfg->rfport);
	wr_ch_lli(chn, "rf_bandwidth",       cfg->bw_hz);
	wr_ch_lli(chn, "sampling_frequency", cfg->fs_hz);

	// Configure RX AGC mode and gain
	if (type == RX) {
		wr_ch_str(chn, "gain_control_mode", "manual");
		wr_ch_str(chn, "hardwaregain", "0");
	}

	// Configure LO channel
	printf("* Acquiring AD9361 %s lo channel\n", type == TX ? "TX" : "RX");
	if (!get_lo_chan(ctx, type, &chn)) { return false; }
	wr_ch_lli(chn, "frequency", cfg->lo_hz);
	return true;
}

/* simple configuration and streaming */
int main (int argc, char **argv)
{
	// Streaming devices
	struct iio_device *tx;
	struct iio_device *rx;

	// Check for overflow/underflow
	int ret;
	uint32_t val;

	// RX and TX sample counters
	size_t nrx = 0;
	size_t ntx = 0;

	// AGC and gain configurations
	struct iio_channel *chn = NULL;

	// Stream configurations
	struct stream_cfg rxcfg;
	struct stream_cfg txcfg;

	// Listen to ctrl+c and ASSERT
	signal(SIGINT, handle_sig);
	signal(SIGTERM, handle_sig);

	// RX stream config
	rxcfg.bw_hz = MHZ(2);   // 2 MHz rf bandwidth
	rxcfg.fs_hz = MHZ(2.5);   // 2.5 MS/s rx sample rate
	rxcfg.lo_hz = GHZ(2.4); // 2.5 GHz rf frequency
	rxcfg.rfport = "A_BALANCED"; // port A (select for rf freq.)

	// TX stream config
	txcfg.bw_hz = MHZ(1.5); // 1.5 MHz rf bandwidth
	txcfg.fs_hz = MHZ(2.5);   // 2.5 MS/s tx sample rate
	txcfg.lo_hz = GHZ(2.4); // 2.5 GHz rf frequency
	txcfg.rfport = "A"; // port A (select for rf freq.)

	// Parameteres for sine wave generation
	const long long f0 = MHZ(0.5);
	const double f0_over_fs = (double)f0/(double)txcfg.fs_hz;
	long long n = 0;

	// Precalculate sine and cosine tables
	printf("* Calculating sine and cosine lookup tables\n");

	sin_table = (int16_t*)malloc(sizeof(int16_t)*txcfg.fs_hz);
	if (!sin_table) {
		perror("Failed to allocate memory for sine lookup table");
		shutdown();
	}

	cos_table = (int16_t*)malloc(sizeof(int16_t)*txcfg.fs_hz);
	if (!cos_table) {
		perror("Failed to allocate memory for cosine lookup table");
		shutdown();
	}

	double temp_I, temp_Q;

#ifdef USE_DITHERING
	for (long long i = 0; i < txcfg.fs_hz; i++, n++) {
		temp_I = cos(2*M_PI * f0_over_fs * (i + noise(1))) * 32767;
		temp_Q = sin(2*M_PI * f0_over_fs * (i + noise(1))) * 32767;
		cos_table[i] = (int16_t)temp_I;
		sin_table[i] = (int16_t)temp_Q;
	}
#else
	for (long long i = 0; i < txcfg.fs_hz; i++, n++) {
		temp_I = cos(2*M_PI*f0_over_fs*i) * 32767;
		temp_Q = sin(2*M_PI*f0_over_fs*i) * 32767;
		cos_table[i] = (int16_t)temp_I;
		sin_table[i] = (int16_t)temp_Q;
	}
#endif

#ifdef DEBUG_SINCOS
	printf("Debuggiranje...\n");
	FILE *file = NULL;
	file = fopen("e:\\sincos.bin", "wb");
	fwrite(cos_table, sizeof(int16_t), txcfg.fs_hz, file);
	fclose(file);
	free(sin_table);
	free(cos_table);
	return 0;
#endif

	printf("* Open RX stream capture file\n");
	rx_capture = fopen(filename, "wb");
	if (!rx_capture) {
		perror("Could not open RX stream capture file");
		shutdown();
	}

	printf("* Acquiring IIO network context\n");
	ASSERT((ctx = iio_create_network_context("192.168.3.2")) && "No context");
	ASSERT(iio_context_get_devices_count(ctx) > 0 && "No devices");

	printf("* Acquiring AD9361 streaming devices\n");
	ASSERT(get_ad9361_stream_dev(ctx, TX, &tx) && "No tx dev found");
	ASSERT(get_ad9361_stream_dev(ctx, RX, &rx) && "No rx dev found");

	printf("* Configuring AD9361 for streaming\n");
	ASSERT(cfg_ad9361_streaming_ch(ctx, &rxcfg, RX, 0) && "RX port 0 not found");
	ASSERT(cfg_ad9361_streaming_ch(ctx, &txcfg, TX, 0) && "TX port 0 not found");

	printf("* Initializing AD9361 IIO streaming channels\n");
	ASSERT(get_ad9361_stream_ch(ctx, RX, rx, 0, &rx0_i) && "RX0 chan i not found");
	ASSERT(get_ad9361_stream_ch(ctx, RX, rx, 1, &rx0_q) && "RX0 chan q not found");
	ASSERT(get_ad9361_stream_ch(ctx, TX, tx, 0, &tx0_i) && "TX0 chan i not found");
	ASSERT(get_ad9361_stream_ch(ctx, TX, tx, 1, &tx0_q) && "TX0 chan q not found");

	/* Set the remote kernel number of buffers */
	/* See for more details: https://ez.analog.com/thread/85655 */
	if (iio_device_set_kernel_buffers_count(rx, RX_BUFFER_COUNT) < 0) {
		fprintf(stderr, "Could not change number of RX kernel buffers\n");
		shutdown();
	}

	if (iio_device_set_kernel_buffers_count(tx, TX_BUFFER_COUNT) < 0) {
		fprintf(stderr, "Could not change number of TX kernel buffers\n");
		shutdown();
	}

	printf("* Enabling IIO streaming channels\n");

	iio_channel_enable(rx0_i);
	iio_channel_enable(rx0_q);

	iio_channel_enable(tx0_i);
	iio_channel_enable(tx0_q);

	printf("* Creating non-cyclic IIO buffers\n");
	rxbuf = iio_device_create_buffer(rx, RX_BUFFER_SIZE, false);
	if (!rxbuf) {
		perror("Could not create RX buffer");
		shutdown();
	}

	txbuf = iio_device_create_buffer(tx, TX_BUFFER_SIZE, false);
	if (!txbuf) {
		perror("Could not create TX buffer");
		shutdown();
	}

	/* Clear overflow/underflow status bits status bits */
	iio_device_reg_write(rx, 0x80000088, 0x6);

	printf("* Starting IO streaming (press CTRL+C to cancel)\n");
	while (!stop)
	{
		ssize_t nbytes_rx = 0, nbytes_tx = 0;
		char *p_dat, *p_end, *p_start;
		ptrdiff_t p_inc;
		ptrdiff_t p_diff;

		// WRITE: Get pointers to TX buf and write IQ to TX buf port 0
		p_inc = iio_buffer_step(txbuf);
		p_end = iio_buffer_end(txbuf);
		for (p_dat = (uint32_t *)iio_buffer_first(txbuf, tx0_i); p_dat < p_end; p_dat += p_inc) {
			// https://wiki.analog.com/resources/eval/user-guides/ad-fmcomms2-ebz/software/basic_iq_datafiles#binary_format
			//*((uint32_t *)p_dat) = (cos_table[n] << 16) | (0xFFFF & sin_table[n]);
			((int16_t *)p_dat)[0] = cos_table[n];
			((int16_t *)p_dat)[1] = sin_table[n];

			n++;
			if (n >= txcfg.fs_hz) n = 0;
		}

		// Schedule TX buffer
		nbytes_tx = iio_buffer_push(txbuf);
		if (nbytes_tx < 0) { printf("Error pushing buf %d\n", (int)nbytes_tx); shutdown(); }

		// READ: Get pointers to RX buf and read IQ from RX buf port 0
		p_inc = iio_buffer_step(rxbuf);
		p_end = iio_buffer_end(rxbuf);
		p_start = (char *)iio_buffer_first(rxbuf, rx0_i);
		p_diff = p_end - p_start;

		// Refill RX buffer
		nbytes_rx = iio_buffer_refill(rxbuf);
		if (nbytes_rx < 0) { printf("Error refilling buf %d\n", (int)nbytes_rx); shutdown(); }

		// WRITE RX -> FILE: Write captured RX samples to a file
		size_t num_of_elements = p_diff / p_inc;
		if (fwrite(p_start, p_inc, num_of_elements, rx_capture) < num_of_elements) {
			printf("Error writing samples to a file\n");
			shutdown();
		}


		// Check for buffer overflow/underflow
		ret = iio_device_reg_read(tx, 0x80000088, &val);
		if (ret)
		{
			fprintf(stderr, "Failed to read status register: %s\n",
					strerror(-ret));
			continue;
		}

		if (val & 0x1u)
				fprintf(stderr, "Underflow detected\n");

		if (val & 0x4u)
				fprintf(stderr, "Overflow detected\n");

		/* Clear bits */
		if (val)
			iio_device_reg_write(tx, 0x80000088, val);


		// Sample counter increment and status output
		nrx += nbytes_rx >> 2;
		ntx += nbytes_tx >> 2;
		if (nrx >= 1024*1024*8 || ntx >= 1024*1024*8) // 16M of data
		{
			fprintf(stderr, "\tRX %8.2f MSmp, TX %8.2f MSmp\n", nrx/1e6, ntx/1e6);
			shutdown();
		}

	}

	shutdown();

	system("PAUSE");
	return 0;
}