#include <SoapySDR/Device.h>
#include <SoapySDR/Formats.h>
#include <SoapySDR/Logger.h>

#include <string.h>
#include <math.h>

#include "logger.h"
#include "config.h"
#include "results.h"
#include "utils.h"

void soapy_log_handler(const SoapySDRLogLevel logLevel, const char *message)
{
	int level;

	switch(logLevel) {
		case SOAPY_SDR_CRITICAL:
		case SOAPY_SDR_FATAL:
			level = LVL_FATAL;
			break;

		case SOAPY_SDR_ERROR:
			level = LVL_ERR;
			break;

		case SOAPY_SDR_WARNING:
			level = LVL_WARN;
			break;

		case SOAPY_SDR_INFO:
			level = LVL_INFO;
			break;

		case SOAPY_SDR_DEBUG:
			level = LVL_DEBUG;
			break;

		default:
			level = LVL_DUMP;
			break;
	}

	LOG(level, "soapy [%d]: %s", logLevel, message);
}


typedef struct {
	SoapySDRDevice *sdr;

	SoapySDRStream *rx_stream;
	SoapySDRStream *tx_stream;
} sdr_ctx_t;


int main(void)
{
	sdr_ctx_t ctx;

	logger_init();

	// set up logging
	SoapySDR_registerLogHandler(soapy_log_handler);
	SoapySDR_setLogLevel(SOAPY_SDR_DEBUG);

	SoapySDRKwargs args;
	memset(&args, 0, sizeof(args));
	SoapySDRKwargs_set(&args, "driver", "sx");
	ctx.sdr = SoapySDRDevice_make(&args);
	SoapySDRKwargs_clear(&args);

	if (ctx.sdr == NULL)
	{
		LOG(LVL_ERR, "SoapySDRDevice_make fail: %s", SoapySDRDevice_lastError());
		return ERR_SOAPY;
	}

	//setup streams
	ctx.rx_stream = SoapySDRDevice_setupStream(ctx.sdr, SOAPY_SDR_RX, SOAPY_SDR_CF32, NULL, 0, NULL);

	if(ctx.rx_stream == NULL) {
		LOG(LVL_ERR, "setupStream fail: %s", SoapySDRDevice_lastError());
		return ERR_SOAPY;
	}

	ctx.tx_stream = SoapySDRDevice_setupStream(ctx.sdr, SOAPY_SDR_TX, SOAPY_SDR_CF32, NULL, 0, NULL);

	if(ctx.tx_stream == NULL) {
		LOG(LVL_ERR, "setupStream fail: %s", SoapySDRDevice_lastError());
		return ERR_SOAPY;
	}

	//apply settings
	if (SoapySDRDevice_setSampleRate(ctx.sdr, SOAPY_SDR_RX, 0, SDR_RX_SAMPLING_RATE) != 0) {
		LOG(LVL_ERR, "setSampleRate fail: %s", SoapySDRDevice_lastError());
		return ERR_SOAPY;
	}
	if (SoapySDRDevice_setFrequency(ctx.sdr, SOAPY_SDR_RX, 0, SDR_RX_FREQ - SDR_RX_IF_SHIFT, NULL) != 0) {
		LOG(LVL_ERR, "setFrequency fail: %s", SoapySDRDevice_lastError());
		return ERR_SOAPY;
	}

	if (SoapySDRDevice_setSampleRate(ctx.sdr, SOAPY_SDR_TX, 0, SDR_TX_SAMPLING_RATE) != 0) {
		LOG(LVL_ERR, "setSampleRate fail: %s", SoapySDRDevice_lastError());
		return ERR_SOAPY;
	}
	if (SoapySDRDevice_setFrequency(ctx.sdr, SOAPY_SDR_TX, 0, SDR_TX_FREQ, NULL) != 0) {
		LOG(LVL_ERR, "setFrequency fail: %s", SoapySDRDevice_lastError());
		return ERR_SOAPY;
	}

	if(SoapySDRDevice_setGain(ctx.sdr, SOAPY_SDR_TX, 0, SDR_GAIN_TX) != 0) {
		LOG(LVL_ERR, "setGain fail: %s", SoapySDRDevice_lastError());
		return ERR_SOAPY;
	}

#define BUFSIZE 300000

#if 0
	if(SoapySDRDevice_activateStream(ctx.sdr, ctx.tx_stream, 0, 0, 0) != 0) {
		LOG(LVL_ERR, "activateStream fail: %s", SoapySDRDevice_lastError());
		return ERR_SOAPY;
	}
#endif

	float complex samples[BUFSIZE];

	float twopi = 6.283185307f;
	float offset_hz = 20e3;
	float fm_osc_freq_hz = 1.0f;
	float fm_osc_dev_hz = 5e3;

	float fm_dphi = twopi * fm_osc_freq_hz / SDR_TX_SAMPLING_RATE;

	float fm_phi = 0.0f;
	float phi = 0.0f;

	int64_t timeNs = 0;

	while(true) {

		for(size_t i = 0; i < BUFSIZE; i++) {
			fm_phi += fm_dphi;
			if(fm_phi > twopi) {
				fm_phi -= twopi;
			}

			float fm_dev_hz = fm_osc_dev_hz * sinf(fm_phi);

			float dphi = twopi * (offset_hz + fm_dev_hz) / SDR_TX_SAMPLING_RATE;

			phi += dphi;
			if(phi > twopi) {
				phi -= twopi;
			}
			samples[i] = cexpf(I * phi);
		}

		timeNs += 1000000000LL + 1000000000LL * BUFSIZE / SDR_TX_SAMPLING_RATE;

		if(SoapySDRDevice_activateStream(ctx.sdr, ctx.tx_stream, 0, 0, 0) != 0) {
			LOG(LVL_ERR, "activateStream fail: %s", SoapySDRDevice_lastError());
			return ERR_SOAPY;
		}

		double tstart = get_hires_time();

		int flags = 0;
		void *buffs[] = {(void*)samples};
		size_t timeout_us = 100000;
		int ret = SoapySDRDevice_writeStream(ctx.sdr, ctx.tx_stream, (const void* const*)buffs, BUFSIZE, &flags, timeNs, timeout_us);

		if(ret <= 0) {
			LOG(LVL_ERR, "writeStream fail: %s", SoapySDRDevice_lastError());
			return ERR_SOAPY;
		}

		double tend_write = get_hires_time();

		if(SoapySDRDevice_deactivateStream(ctx.sdr, ctx.tx_stream, 0, 0) != 0) {
			LOG(LVL_ERR, "deactivateStream fail: %s", SoapySDRDevice_lastError());
			return ERR_SOAPY;
		}

		double tend = get_hires_time();

		LOG(LVL_INFO, "write duration: %.3f ms", (tend_write - tstart) * 1000);
		LOG(LVL_INFO, "total duration: %.3f ms", (tend - tstart) * 1000);


		fsleep(1);
	}

	return OK;
}