/*
* Copyright 2026 Wavelet Lab <info@wavelet-lab.com>
*
* SPDX-License-Identifier: AGPL-3.0+
*
* This software is distributed under the terms of the GNU Affero Public License.
* See the COPYING file in the main directory for details.
*
* This use of this software may be subject to additional restrictions.
* See the LEGAL file in the main directory for details.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Affero General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU Affero General Public License for more details.
*
* You should have received a copy of the GNU Affero General Public License
* along with this program.  If not, see <http://www.gnu.org/licenses/>.
*
*/

#include <stdint.h>
#include <string.h>
#include <stdlib.h>
#include <string>
#include <array>
#include <cmath>

#include "osmocom/core/utils.h"
#include "Threads.h"
#include "USDRDevice.h"

#include <Logger.h>
#include <errno.h>

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

static const devname_map_t devname_map = {
	{ "usdr", USDR },
	{ "xsdr", XSDR },
	{ "ssdr", SSDR },
};

using namespace std::chrono_literals;

static const dev_map_t dev_map = {
	/* Unknown device type */
	{ UNKNOWN_DEV, { rx_gain_range: { 0, 0 }, tx_gain_range: { 0, 0 }, tx_offset: 0s, desc_str: "Unknown device" } },
	/* uSDR, 16 samples at 1083333.33333 sps corresponds to 14.769 us */
	{ USDR, { rx_gain_range: { 0, 30 }, tx_gain_range: { 0, 30 }, tx_offset: 14.769us, desc_str: "uSDR" } },
	/* xSDR, 76 samples at 1083333.33333 sps corresponds to 70.154 us */
	{ XSDR, { rx_gain_range: { 0, 30 }, tx_gain_range: { 0, 30 }, tx_offset: 70.154us, desc_str: "xSDR" } },
	/* sSDR, 76 samples at 1083333.33333 sps corresponds to 70.154 us */
	{ SSDR, { rx_gain_range: { 0, 30 }, tx_gain_range: { 0, 30 }, tx_offset: 70.154us, desc_str: "sSDR" } },
};


/* Device model prefixes as presented by USDR API */
#define DM_SDR(idx) "/dm/sdr/" OSMO_STRINGIFY_VAL(idx)	/* /dm/sdr/<idx> - DM prefix where <idx> is the SDR index */
#define DM_SDR_TX_FREQ(idx) DM_SDR(idx) "/tx/frequency"	/* /dm/sdr/<idx>/tx/frequency - path to set Tx frequency */
#define DM_SDR_RX_FREQ(idx) DM_SDR(idx) "/rx/frequency"	/* /dm/sdr/<idx>/rx/frequency - path to set Rx frequency */
#define DM_SDR_TX_GAIN(idx) DM_SDR(idx) "/tx/gain"	/* /dm/sdr/<idx>/tx/gain - path to set Tx gain */
#define DM_SDR_RX_GAIN(idx) DM_SDR(idx) "/rx/gain"	/* /dm/sdr/<idx>/rx/gain - path to set Rx gain */
#define DM_SDR_TX_BW(idx) DM_SDR(idx) "/tx/bandwidth"	/* /dm/sdr/<idx>/tx/bandwidth - path to set Tx bandwidth */
#define DM_SDR_RX_BW(idx) DM_SDR(idx) "/rx/bandwidth"	/* /dm/sdr/<idx>/rx/bandwidth - path to set Rx bandwidth */

/* Low level prefixes as presented by USDR API */
#define LL_DEVICE_NAME "/ll/device/name"		/* /ll/device/name - path to get the device name */
#define LL_STX(idx) "/ll/stx/" OSMO_STRINGIFY_VAL(idx)	/* /ll/stx/<idx> - path to set stream TX with index <idx> */
#define LL_SRX(idx) "/ll/srx/" OSMO_STRINGIFY_VAL(idx)	/* /ll/srx/<idx> - path to set stream RX with index <idx> */

/* Convert frequency in Hz to MHz for logging purposes */
#define FREQ_TO_MHZ(freq) ((freq) * 1e-6)

/* Device samples format */
#define USDR_FMT_CI16 "ci16"	/* Complex int16 */

/* Device sync stream types */
#define USDR_SYNC_RX "rx"	/* Sync stream starts after Rx reception */

/* Number of samples per burst */
#define GSM_BURST 625

/* Channel mask for USDR device */
#define USDR_CHANNEL_MASK(chan) (1ULL << (chan))

/**
 * Number of initial samples to skip after starting the device, to allow it to stabilize and avoid sending
 * corrupted samples to the air. The value of 20000 samples corresponds to approximately 18.5 ms
 * at a sample rate of 1083333.33333 sps, which should be sufficient for the device to stabilize.
 */
#define USDR_SKIP_TX_SAMPLES 20000

/* Timeout for sending samples to USDR device, in milliseconds */
#define USDR_SEND_TIMEOUT_MS 5000

/* Implementation of helper functions */

static enum usdr_dev_type get_dev_type_by_name(const char* name)
{
	if (!name) {
		LOGC(DDEV, WARNING) << "No device name provided";
		return UNKNOWN_DEV;
	}

	const auto it = devname_map.find(name);
	if (it != devname_map.end())
		return it->second;

	LOGC(DDEV, WARNING) << "Could not detect device type from name: " << name;

	return UNKNOWN_DEV;
}

static std::reference_wrapper<const struct dev_desc> get_dev_desc_by_type(enum usdr_dev_type type)
{
	const auto it = dev_map.find(type);
	if (it != dev_map.end()) {
		return std::cref(it->second);
	}

	LOGC(DDEV, WARNING) << "Unknown device type: " << type;

	return std::cref(dev_map.at(UNKNOWN_DEV));
}

static int parse_config(const char *line, const char *argument, int default_value)
{
	const char *arg_found = strstr(line, argument);
	if (!arg_found)
		return default_value;

	const char *qe_pos = strchr(arg_found, '=');
	if (!qe_pos)
		return default_value;

	int res = strtol(qe_pos + 1, nullptr, 10);
	if (res == 0 && errno)
		return default_value;

	return res;
}

static std::string get_err_description(int err_code)
{
	return "res: " + std::to_string(err_code)
		+ " (" + std::error_code(std::abs(err_code), std::generic_category()).message() + ")";
}

/* Wrapper functions for USDR API calls with error logging */

/**
 * USDR log callback function to redirect USDR logs to Osmocom logging framework
 * You can set the log level using vty command "logging level devdrv <level>",
 * where <level> is one of the following: fatal, error, notice, info, debug
 */
static int usdr_log_callback(uintptr_t param, unsigned sevirity, const char* log) {
	/* map usdr specific severity levels */
	static const int sevirity_map[7] = {
		[USDR_LOG_ERROR] = LOGL_FATAL,
		[USDR_LOG_CRITICAL_WARNING] = LOGL_ERROR,
		[USDR_LOG_WARNING] = LOGL_NOTICE,
		[USDR_LOG_INFO] = LOGL_INFO,
		[USDR_LOG_NOTE] = LOGL_INFO,
		[USDR_LOG_DEBUG] = LOGL_DEBUG,
		[USDR_LOG_TRACE] = LOGL_DEBUG,
	};
	/* protect against future higher severity level values (lower importance) */
	if ((unsigned int) sevirity >= ARRAY_SIZE(sevirity_map))
		sevirity = ARRAY_SIZE(sevirity_map)-1;

	LOGLV(DDEVDRV, sevirity_map[sevirity]) << log;

	return 0;
}

static int usdr_set_parameter(pdm_dev_t dev, const char *path, uint64_t val)
{
	const int res = usdr_dme_set_uint(dev, path, val);
	if (res) {
		LOGC(DDEV, ERROR) << "Error setting " << (path ? path : "(null)") << " to " << val
			<< " " << get_err_description(res);
	}

	return res;
}

static int usdr_stream_create(pdm_dev_t device, const char *sobj, const char *dformat,
			      logical_ch_msk_t channels, unsigned pktsyms, pusdr_dms_t *outu)
{
	const int res = usdr_dms_create(device, sobj, dformat, channels, pktsyms, outu);
	if (res) {
		LOGC(DDEV, ERROR) << "Error creating stream for " << (sobj ? sobj : "(null)")
			<< " " << get_err_description(res);
	}

	return res;
}

static int usdr_stream_info(pusdr_dms_t stream, usdr_dms_nfo_t *nfo)
{
	const int res = usdr_dms_info(stream, nfo);
	if (res) {
		LOGC(DDEV, ERROR) << "Error getting stream info for stream " << get_err_description(res);
	}

	return res;
}

static int usdr_stream_op(pusdr_dms_t stream, unsigned command, dm_time_t tm)
{
	const int res = usdr_dms_op(stream, command, tm);
	if (res) {
		LOGC(DDEV, ERROR) << "Error doing stream op command: " << command
			<< " " << get_err_description(res);
	}

	return res;
}

/* Implementation of USDRDevice class methods */

USDRDevice::USDRDevice(InterfaceType iface, const struct trx_cfg *cfg) :
	RadioDevice(iface, cfg), devDesc(std::cref(dev_map.at(UNKNOWN_DEV)))
{
	LOGC(DDEV, INFO) << "Creating USDR device... SPS=" << cfg->tx_sps << "/" << cfg->rx_sps;

	/* Resize the vectors for gains and frequencies based on the number of channels in the configuration */
	rx_gains.resize(chans);
	tx_gains.resize(chans);

	rx_freqs.resize(chans);
	tx_freqs.resize(chans);
}

USDRDevice::~USDRDevice()
{
	if (!dev)
		return;

	/* Stop the DMS streams and close device if the device opened */
	stop();
	usdr_dmd_close(dev);
}

int USDRDevice::open()
{
	int res;
	uint64_t val;
	std::array<pusdr_dms_t, 2> ped = { usds_rx, usds_tx };

	const unsigned samples = GSM_BURST * tx_sps;
	const double rate = GSMRATE * tx_sps;
	const char *args = cfg->dev_args ? cfg->dev_args : "";
	const int loglevel = parse_config(args, "loglevel", 0);

	LOGC(DDEV, INFO) << "Opening USDR device ARGS: " << args;

	usdrlog_set_log_op(&usdr_log_callback);
	usdrlog_setlevel(nullptr, loglevel);

	/* Create the USDR device */
	res = usdr_dmd_create_string(args, &dev);
	if (res) {
		LOGC(DDEV, ERROR) << "Failed to create USDR device with args: " << args
			<< " " << get_err_description(res);
		return res;
	}

	/* Set the sample rate */
	res = usdr_dmr_rate_set(dev, nullptr, static_cast<unsigned int>(std::lround(rate)));
	if (res) {
		LOGC(DDEV, ERROR) << "Failed to set USDR sample rate to " << rate
			<< " " << get_err_description(res);
		goto out_close;
	}

	/* Get device name if available */
	res = usdr_dme_get_uint(dev, LL_DEVICE_NAME, &val);
	if (res == 0 && val != 0) {
		const char *device = reinterpret_cast<const char *>(static_cast<uintptr_t>(val));

		LOGC(DDEV, INFO) << "Device name is \"" << device << "\"";

		this->devDesc = get_dev_desc_by_type(get_dev_type_by_name(device));

		/* convert time offset in seconds to samples using the actual sample rate */
		timeTxCorr = static_cast<TIMESTAMP>(std::lround(this->devDesc.get().tx_offset.count() * rate));
		LOGC(DDEV, INFO) << "Time TX correction for device \"" << device << "\" is "
			<< timeTxCorr << " samples";
	}

	/* Set TX and RX gains */
	setPowerAttenuation(0, 0);
	setRxGain((minRxGain() + maxRxGain()) / 2, 0);

	/* Set TX and RX bandwidth */
	usdr_set_parameter(dev, DM_SDR_TX_BW(0), 1'000'000ULL);
	usdr_set_parameter(dev, DM_SDR_RX_BW(0), 500'000ULL);

	/* Create DMS streams for Tx */
	res = usdr_stream_create(dev, LL_STX(0), USDR_FMT_CI16, USDR_CHANNEL_MASK(0), samples, &usds_tx);
	if (res)
		goto out_close;

	res = usdr_stream_info(usds_tx, &snfo_tx);
	if (res)
		goto out_close;

	/* Create DMS streams for Rx */
	res = usdr_stream_create(dev, LL_SRX(0), USDR_FMT_CI16, USDR_CHANNEL_MASK(0), samples, &usds_rx);
	if (res)
		goto out_close;

	res = usdr_stream_info(usds_rx, &snfo_rx);
	if (res)
		goto out_close;

	/* Stop the DMS streams to prevent them from running before start() is called */
	res = usdr_stream_op(usds_rx, USDR_DMS_STOP, 0);
	if (res)
		goto out_close;

	res = usdr_stream_op(usds_tx, USDR_DMS_STOP, 0);
	if (res)
		goto out_close;

	/* Sync the streams */
	res = usdr_dms_sync(dev, USDR_SYNC_RX, ped.size(), ped.data());
	if (res) {
		LOGC(DDEV, ERROR) << "Failed to sync stream " << get_err_description(res);
		goto out_close;
	}

	actualSampleRate = rate;
	started = false;

	return NORMAL;

out_close:
	usdr_dmd_close(dev);
	dev = nullptr;

	return res;
}

bool USDRDevice::start()
{
	int res;

	LOGC(DDEV, INFO) << "Starting USDR...";
	if (started) {
		LOGC(DDEV, ERROR) << "USDR device is already started";
		return false;
	}

	res = usdr_stream_op(usds_tx, USDR_DMS_START, 0);
	if (res)
		return false;

	res = usdr_stream_op(usds_rx, USDR_DMS_START, 0);
	if (res) {
		usdr_stream_op(usds_tx, USDR_DMS_STOP, 0);
		return false;
	}

	started = true;

	return true;
}

bool USDRDevice::stop()
{
	LOGC(DDEV, INFO) << "Stopping USDR...";

	if (!started) {
		LOGC(DDEV, ERROR) << "USDR device is not started";
		return false;
	}

	usdr_stream_op(usds_rx, USDR_DMS_STOP, 0);
	usdr_stream_op(usds_tx, USDR_DMS_STOP, 0);

	started = false;

	return true;
}

int USDRDevice::readSamples(std::vector<short *> &bufs, int len, bool *overrun,
			    TIMESTAMP timestamp, bool *underrun)
{
	struct usdr_dms_recv_nfo ri;

	if (bufs.size() != chans) {
		LOGC(DDEV, ERROR) << "Invalid channel combination " << bufs.size();
		return -1;
	}

	const int res = usdr_dms_recv(usds_rx, (void **)&bufs[0], len, &ri);
	if (res) {
		LOGC(DDEV, ERROR) << "usdr_dms_recv failed res " << res << " dev TS " << ri.fsymtime
			<< " req TS" << timestamp;
		return -1;
	}

	const unsigned diff = ri.fsymtime - ts_last_recv;
	ts_last_recv = ri.fsymtime + len;
	if (ts_initial == 0)
		ts_initial = ri.fsymtime;

	LOGC(DDEV, DEBUG) << "usdr_dms_recv TS=" << ri.fsymtime << " diff=" << diff;

	if (underrun)
		*underrun = (diff != 0) ? true : false;
	if (overrun)
		*overrun = false;

	return len;

}

int USDRDevice::writeSamples(std::vector<short *> &bufs, int len, bool *underrun, TIMESTAMP timestamp)
{
	if (bufs.size() != chans) {
		LOGC(DDEV, ERROR) << "Invalid channel combination " << bufs.size();
		return -1;
	}

	/* skip USDR_SKIP_TX_SAMPLES samples after start */
	if (timestamp < USDR_SKIP_TX_SAMPLES)
		return len;

	const TIMESTAMP adj_timestamp = timestamp - timeTxCorr;
	LOGC(DDEV, DEBUG) << "writeSamples TS=" << adj_timestamp << " len=" << len;

	const int res = usdr_dms_send(usds_tx, (const void**)&bufs[0], len, adj_timestamp, USDR_SEND_TIMEOUT_MS);
	if (res) {
		LOGC(DDEV, ERROR) << "Error sending samples: len=" << len << " ts=" << timestamp
			<< " adj_ts=" << adj_timestamp << " " << get_err_description(res);
		return 0;
	}

	return len;
}

bool USDRDevice::setTxFreq(double wFreq, size_t chan)
{
	LOGC(DDEV, INFO) << "Setting TX frequency to " << FREQ_TO_MHZ(wFreq) << " MHz at channel " << chan;
	if (chan >= tx_freqs.size()) {
		LOGC(DDEV, ALERT) << "Requested non-existent channel " << chan;
		return false;
	}

	const int res = usdr_set_parameter(dev, DM_SDR_TX_FREQ(0), static_cast<uint64_t>(std::lround(wFreq)));
	if (res)
		return false;

	tx_freqs[chan] = wFreq;

	return true;
}

bool USDRDevice::setRxFreq(double wFreq, size_t chan)
{
	LOGC(DDEV, INFO) << "Setting RX frequency to " << FREQ_TO_MHZ(wFreq) << " MHz at channel " << chan;
	if (chan >= rx_freqs.size()) {
		LOGC(DDEV, ALERT) << "Requested non-existent channel " << chan;
		return false;
	}

	const int res = usdr_set_parameter(dev, DM_SDR_RX_FREQ(0), static_cast<uint64_t>(std::lround(wFreq)));
	if (res)
		return false;

	rx_freqs[chan] = wFreq;

	return true;
}

TIMESTAMP USDRDevice::initialWriteTimestamp()
{
	if (rx_sps == tx_sps)
		return initialReadTimestamp();
	else
		return initialReadTimestamp() * tx_sps;
}

double USDRDevice::setRxGain(double dB, size_t chan)
{
	if (dB > maxRxGain())
		dB = maxRxGain();
	if (dB < minRxGain())
		dB = minRxGain();

	LOGC(DDEV, INFO) << "Setting RX gain to " << dB << " dB at channel " << chan;
	if (chan >= rx_gains.size()) {
		LOGC(DDEV, ALERT) << "Requested non-existent channel " << chan;
		return 0.0;
	}

	const int res = usdr_set_parameter(dev, DM_SDR_RX_GAIN(0), dB);
	if (!res)
		rx_gains[chan] = dB;

	return rx_gains[chan];
}

double USDRDevice::getRxGain(size_t chan)
{
	if (chan >= rx_gains.size()) {
		LOGC(DDEV, ALERT) << "Requested non-existent channel " << chan;
		return 0.0;
	}

	return rx_gains[chan];
}

double USDRDevice::maxRxGain()
{
	return devDesc.get().rx_gain_range.max;
}

double USDRDevice::minRxGain()
{
	return devDesc.get().rx_gain_range.min;
}

double USDRDevice::rssiOffset(size_t chan)
{
	/* Dummy implementation for now, always return -20 dB */
	return -20;
}

int USDRDevice::getNominalTxPower(size_t chan)
{
	return 0;
}

bool USDRDevice::setRxAntenna(const std::string &ant, size_t chan)
{
	LOGC(DDEV, INFO) << "Setting RX antenna to " << ant << " at channel " << chan;
	if (chan >= rx_paths.size()) {
		LOGC(DDEV, ALERT) << "Requested non-existent channel " << chan;
		return false;
	}

	rx_paths[chan] = ant;

	return true;
}

std::string USDRDevice::getRxAntenna(size_t chan)
{
	if (chan >= rx_paths.size()) {
		LOGC(DDEV, ALERT) << "Requested non-existent channel " << chan;
		return "";
	}

	return rx_paths[chan];
}

bool USDRDevice::setTxAntenna(const std::string &ant, size_t chan)
{
	if (chan >= tx_paths.size()) {
		LOGC(DDEV, ALERT) << "Requested non-existent channel " << chan;
		return false;
	}

	LOGC(DDEV, INFO) << "Setting TX antenna at channel " << chan << " to " << ant;

	tx_paths[chan] = ant;

	return true;
}

std::string USDRDevice::getTxAntenna(size_t chan)
{
	if (chan >= tx_paths.size()) {
		LOGC(DDEV, ALERT) << "Requested non-existent channel " << chan;
		return "";
	}

	return tx_paths[chan];
}

GSM::Time USDRDevice::minLatency()
{
	return GSM::Time(6, 7);
}

double USDRDevice::getTxFreq(size_t chan)
{
	if (chan >= tx_freqs.size()) {
		LOGC(DDEV, ALERT) << "Requested non-existent channel " << chan;
		return 0;
	}

	return tx_freqs[chan];
}

double USDRDevice::getRxFreq(size_t chan)
{
	if (chan >= rx_freqs.size()) {
		LOGC(DDEV, ALERT) << "Requested non-existent channel " << chan;
		return 0;
	}

	return rx_freqs[chan];
}

double USDRDevice::setPowerAttenuation(int atten, size_t chan)
{
	if (atten > maxTxGain())
		atten = maxTxGain();
	if (atten < minTxGain())
		atten = minTxGain();

	LOGC(DDEV, INFO) << "Setting power attenuation to " << atten << " dB at channel " << chan;
	if (chan >= tx_gains.size()) {
		LOGC(DDEV, ALERT) << "Requested non-existent channel " << chan;
		return 0;
	}

	const int res = usdr_set_parameter(dev, DM_SDR_TX_GAIN(0), atten);
	if (!res)
		tx_gains[chan] = atten;

	return tx_gains[chan];
}

double USDRDevice::getPowerAttenuation(size_t chan)
{
	if (chan >= tx_gains.size()) {
		LOGC(DDEV, ALERT) << "Requested non-existent channel " << chan;
		return 0;
	}

	return tx_gains[chan];
}

double USDRDevice::maxTxGain()
{
	return devDesc.get().tx_gain_range.max;
}

double USDRDevice::minTxGain()
{
	return devDesc.get().tx_gain_range.min;
}

RadioDevice *RadioDevice::make(InterfaceType iface, const struct trx_cfg *cfg)
{
	if (!cfg) {
		LOGC(DDEV, ERROR) << "No configuration provided for USDR device";
		return nullptr;
	}

	return new USDRDevice(iface, cfg);
}