/**
 * Copyright 2013-2023 Software Radio Systems Limited
 *
 * This file is part of srsRAN.
 *
 * srsRAN 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.
 *
 * srsRAN 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.
 *
 * A copy of the GNU Affero General Public License can be found in
 * the LICENSE file in the top-level directory of this distribution
 * and at http://www.gnu.org/licenses/.
 *
 */

#include <pthread.h>
#include <string.h>
#include <sys/time.h>
#include <unistd.h>

#include "rf_helper.h"
#include "rf_plugin.h"
#include "rf_soapy_imp.h"
#include "srsran/phy/common/phy_common.h"
#include "srsran/phy/utils/debug.h"
#include "srsran/phy/utils/vector.h"

#include <SoapySDR/Device.h>
#include <SoapySDR/Formats.h>
#include <SoapySDR/Logger.h>
#include <SoapySDR/Time.h>
#include <SoapySDR/Types.h>
#include <SoapySDR/Version.h>

#define HAVE_ASYNC_THREAD 0

#define STOP_STREAM_BEFORE_RATE_CHANGE 0
#define USE_TX_MTU 0
#define SET_RF_BW 0

#define PRINT_RX_STATS 0
#define PRINT_TX_STATS 0

typedef struct {
  char*            devname;
  SoapySDRKwargs   args;
  SoapySDRDevice*  device;
  SoapySDRRange*   ranges;
  SoapySDRStream*  rxStream;
  SoapySDRStream*  txStream;
  bool             tx_stream_active;
  bool             rx_stream_active;
  srsran_rf_info_t info;
  double           tx_rate;
  size_t           rx_mtu, tx_mtu;
  size_t           num_rx_channels;
  size_t           num_tx_channels;

  srsran_rf_error_handler_t soapy_error_handler;
  void*                     soapy_error_handler_arg;

  bool      async_thread_running;
  pthread_t async_thread;

  uint32_t num_time_errors;
  uint32_t num_lates;
  uint32_t num_overflows;
  uint32_t num_underflows;
  uint32_t num_other_errors;
  uint32_t num_stream_curruption;
} rf_soapy_handler_t;

cf_t zero_mem[64 * 1024];

static void log_overflow(rf_soapy_handler_t* h)
{
  if (h->soapy_error_handler) {
    srsran_rf_error_t error;
    bzero(&error, sizeof(srsran_rf_error_t));
    error.type = SRSRAN_RF_ERROR_OVERFLOW;
    h->soapy_error_handler(h->soapy_error_handler_arg, error);
  } else {
    h->num_overflows++;
  }
}

static void log_late(rf_soapy_handler_t* h, bool is_rx)
{
  if (h->soapy_error_handler) {
    srsran_rf_error_t error;
    bzero(&error, sizeof(srsran_rf_error_t));
    error.opt  = is_rx ? 1 : 0;
    error.type = SRSRAN_RF_ERROR_LATE;
    h->soapy_error_handler(h->soapy_error_handler_arg, error);
  } else {
    h->num_lates++;
  }
}

static void log_underflow(rf_soapy_handler_t* h)
{
  if (h->soapy_error_handler) {
    srsran_rf_error_t error;
    bzero(&error, sizeof(srsran_rf_error_t));
    error.type = SRSRAN_RF_ERROR_UNDERFLOW;
    h->soapy_error_handler(h->soapy_error_handler_arg, error);
  } else {
    h->num_underflows++;
  }
}

#if HAVE_ASYNC_THREAD
static void* async_thread(void* h)
{
  rf_soapy_handler_t* handler = (rf_soapy_handler_t*)h;

  while (handler->async_thread_running) {
    int        ret       = 0;
    size_t     chanMask  = 0;
    int        flags     = 0;
    const long timeoutUs = 400000; // arbitrarily chosen
    long long  timeNs;

    ret = SoapySDRDevice_readStreamStatus(handler->device, handler->txStream, &chanMask, &flags, &timeNs, timeoutUs);
    if (ret == SOAPY_SDR_TIME_ERROR) {
      // this is a late
      log_late(handler, false);
    } else if (ret == SOAPY_SDR_UNDERFLOW) {
      log_underflow(handler);
    } else if (ret == SOAPY_SDR_OVERFLOW) {
      log_overflow(handler);
    } else if (ret == SOAPY_SDR_TIMEOUT) {
      // this is a timeout of the readStreamStatus call, ignoring it ..
    } else if (ret == SOAPY_SDR_NOT_SUPPORTED) {
      // stopping async thread
      ERROR("Receiving async metadata not supported by device. Exiting thread.");
      handler->async_thread_running = false;
    } else {
      ERROR("Error while receiving aync metadata: %s (%d), flags=%d, channel=%zu, timeNs=%lld",
            SoapySDR_errToStr(ret),
            ret,
            flags,
            chanMask,
            timeNs);
      handler->async_thread_running = false;
    }
  }
  return NULL;
}
#endif

int soapy_error(void* h)
{
  return 0;
}

void rf_soapy_get_freq_range(void* h)
{
  // not supported
}

void rf_soapy_suppress_handler(const char* x)
{
  // not supported
}

void rf_soapy_msg_handler(const char* msg)
{
  // not supported
}

void rf_soapy_suppress_stdout(void* h)
{
  // not supported
}

void rf_soapy_register_error_handler(void* h, srsran_rf_error_handler_t new_handler, void* arg)
{
  rf_soapy_handler_t* handler      = (rf_soapy_handler_t*)h;
  handler->soapy_error_handler     = new_handler;
  handler->soapy_error_handler_arg = arg;
}

const char* rf_soapy_devname(void* h)
{
  rf_soapy_handler_t* handler = (rf_soapy_handler_t*)h;
  return handler->devname;
}

static bool rf_soapy_rx_wait_lo_locked(rf_soapy_handler_t* handler)
{
  char* ret = SoapySDRDevice_readChannelSensor(handler->device, SOAPY_SDR_RX, 0, "lo_locked");
  if (ret != NULL) {
    return (strcmp(ret, "true") == 0 ? true : false);
  }
  return true;
}

void rf_soapy_calibrate_tx(void* h)
{
  rf_soapy_handler_t* handler   = (rf_soapy_handler_t*)h;
  double              actual_bw = SoapySDRDevice_getBandwidth(handler->device, SOAPY_SDR_TX, 0);
  char                str_buf[25];
  snprintf(str_buf, sizeof(str_buf), "%f", actual_bw);
  str_buf[24] = 0;
  SoapySDRDevice_writeSetting(handler->device, "CALIBRATE_TX", str_buf);
}

int rf_soapy_start_rx_stream(void* h, bool now)
{
  rf_soapy_handler_t* handler = (rf_soapy_handler_t*)h;
  if (handler->rx_stream_active == false) {
    if (SoapySDRDevice_activateStream(handler->device, handler->rxStream, 0, 0, 0) != 0) {
      printf("Error starting Rx streaming.\n");
      return SRSRAN_ERROR;
    }

    handler->rx_stream_active = true;
  }
  return SRSRAN_SUCCESS;
}

int rf_soapy_start_tx_stream(void* h)
{
  rf_soapy_handler_t* handler = (rf_soapy_handler_t*)h;
  if (handler->tx_stream_active == false) {
    if (SoapySDRDevice_activateStream(handler->device, handler->txStream, 0, 0, 0) != 0) {
      printf("Error starting Tx streaming.\n");
      return SRSRAN_ERROR;
    }
    handler->tx_stream_active = true;
  }
  return SRSRAN_SUCCESS;
}

int rf_soapy_stop_rx_stream(void* h)
{
  rf_soapy_handler_t* handler = (rf_soapy_handler_t*)h;
  if (SoapySDRDevice_deactivateStream(handler->device, handler->rxStream, 0, 0) != 0) {
    printf("Error deactivating Rx streaming.\n");
    return SRSRAN_ERROR;
  }

  handler->rx_stream_active = false;
  return SRSRAN_SUCCESS;
}

int rf_soapy_stop_tx_stream(void* h)
{
  rf_soapy_handler_t* handler = (rf_soapy_handler_t*)h;
  if (SoapySDRDevice_deactivateStream(handler->device, handler->txStream, 0, 0) != 0) {
    printf("Error deactivating Tx streaming.\n");
    return SRSRAN_ERROR;
  }

  handler->tx_stream_active = false;
  return SRSRAN_SUCCESS;
}

void rf_soapy_flush_buffer(void* h)
{
  int   n;
  cf_t  dummy[1024];
  void* data[SRSRAN_MAX_CHANNELS] = {};

  for (int i = 0; i < SRSRAN_MAX_CHANNELS; i++) {
    data[i] = dummy;
  }

  do {
    n = rf_soapy_recv_with_time_multi(h, data, sizeof(dummy), 0, NULL, NULL);
  } while (n > 0);
}

bool rf_soapy_has_rssi(void* h)
{
  // TODO: implement rf_soapy_has_rssi()
  return false;
}

float rf_soapy_get_rssi(void* h)
{
  printf("TODO: implement rf_soapy_get_rssi()\n");
  return 0.0;
}

int rf_soapy_open_multi(char* args, void** h, uint32_t num_requested_channels)
{
  size_t length;

  // Let Soapy pick the device if no arguments are passed
  SoapySDRKwargs* soapy_args =
      args == NULL ? SoapySDRDevice_enumerate(NULL, &length) : SoapySDRDevice_enumerateStrArgs(args, &length);

  if (length == 0) {
    printf("No Soapy devices found.\n");
    SoapySDRKwargsList_clear(soapy_args, length);
    return SRSRAN_ERROR;
  }

  // Print connected devices
  for (size_t i = 0; i < length; i++) {
    printf("Soapy has found device #%d: ", (int)i);
    for (size_t j = 0; j < soapy_args[i].size; j++) {
      printf("%s=%s, ", soapy_args[i].keys[j], soapy_args[i].vals[j]);
    }
    printf("\n");
  }

  // Select Soapy device by id
  int dev_id = 0;
  if (args != NULL) {
    const char dev_arg[] = "id=";
    char*      dev_ptr   = strstr(args, dev_arg);
    if (dev_ptr) {
      char dev_str[64] = {0};
      copy_subdev_string(dev_str, dev_ptr + strnlen(dev_arg, 64));
      dev_id = strtol(dev_str, NULL, 0);
      if (dev_id < 0 || dev_id > 10) {
        ERROR("Failed to set device. Using 0 as default.");
        dev_id = 0;
      }
      remove_substring(args, dev_arg);
      remove_substring(args, dev_str);
    }
  }

  // select last device if dev_id exceeds available devices
  dev_id = SRSRAN_MIN(dev_id, length - 1);
  printf("Selecting Soapy device: %d\n", dev_id);

  // With the Lime we are better off using LTE sample rates
  const char* devname = SoapySDRKwargs_get(&soapy_args[dev_id], "name");
  if (devname != NULL && strstr(devname, "Lime") != NULL && srsran_symbol_size_is_standard() == false) {
    printf("\033[0;31mDetected LimeSDR. Consider using LTE rates for better RF performance.\nEither compile with "
           "\'-DUSE_LTE_RATES=True\' or start srsENB/srsUE with \'--expert.lte_sample_rates=true\'\033[0m\n");
  }

  // Now make the device
  SoapySDRDevice* sdr = SoapySDRDevice_make(&(soapy_args[dev_id]));
  if (sdr == NULL) {
    printf("Failed to create Soapy object\n");
    return SRSRAN_ERROR;
  }
  SoapySDRKwargsList_clear(soapy_args, length);

  // create handler
  rf_soapy_handler_t* handler = (rf_soapy_handler_t*)malloc(sizeof(rf_soapy_handler_t));
  bzero(handler, sizeof(rf_soapy_handler_t));
  *h                        = handler;
  handler->device           = sdr;
  handler->tx_stream_active = false;
  handler->rx_stream_active = false;
  handler->devname          = DEVNAME_SOAPY;

  // create stream args from device args
  SoapySDRKwargs stream_args = {};
#if SOAPY_SDR_API_VERSION >= 0x00060000
  stream_args = SoapySDRKwargs_fromString(args);
#endif

  // Setup Rx streamer
  size_t num_available_channels = SoapySDRDevice_getNumChannels(handler->device, SOAPY_SDR_RX);
  if ((num_available_channels > 0) && (num_requested_channels > 0)) {
    handler->num_rx_channels = SRSRAN_MIN(num_available_channels, num_requested_channels);
    size_t rx_channels[handler->num_rx_channels];
    for (int i = 0; i < handler->num_rx_channels; i++) {
      rx_channels[i] = i;
    }
    printf("Setting up Rx stream with %zd channel(s)\n", handler->num_rx_channels);
#if SOAPY_SDR_API_VERSION < 0x00080000
    if (SoapySDRDevice_setupStream(handler->device,
                                   &handler->rxStream,
                                   SOAPY_SDR_RX,
                                   SOAPY_SDR_CF32,
                                   rx_channels,
                                   handler->num_rx_channels,
                                   &stream_args) != 0) {
#else
    handler->rxStream = SoapySDRDevice_setupStream(
        handler->device, SOAPY_SDR_RX, SOAPY_SDR_CF32, rx_channels, handler->num_rx_channels, &stream_args);
    if (handler->rxStream == NULL) {
#endif
      printf("Rx setupStream fail: %s\n", SoapySDRDevice_lastError());
      return SRSRAN_ERROR;
    }
    handler->rx_mtu = SoapySDRDevice_getStreamMTU(handler->device, handler->rxStream);
  }

  // Setup Tx streamer
  num_available_channels = SoapySDRDevice_getNumChannels(handler->device, SOAPY_SDR_TX);
  if ((num_available_channels > 0) && (num_requested_channels > 0)) {
    handler->num_tx_channels = SRSRAN_MIN(num_available_channels, num_requested_channels);
    size_t tx_channels[handler->num_tx_channels];
    for (int i = 0; i < handler->num_tx_channels; i++) {
      tx_channels[i] = i;
    }
    printf("Setting up Tx stream with %zd channel(s)\n", handler->num_tx_channels);
#if SOAPY_SDR_API_VERSION < 0x00080000
    if (SoapySDRDevice_setupStream(handler->device,
                                   &handler->txStream,
                                   SOAPY_SDR_TX,
                                   SOAPY_SDR_CF32,
                                   tx_channels,
                                   handler->num_tx_channels,
                                   &stream_args) != 0) {
#else
    handler->txStream = SoapySDRDevice_setupStream(
        handler->device, SOAPY_SDR_TX, SOAPY_SDR_CF32, tx_channels, handler->num_tx_channels, &stream_args);
    if (handler->txStream == NULL) {
#endif
      printf("Tx setupStream fail: %s\n", SoapySDRDevice_lastError());
      return SRSRAN_ERROR;
    }
    handler->tx_mtu = SoapySDRDevice_getStreamMTU(handler->device, handler->txStream);
  }

  // init rx/tx rate to lowest LTE rate to avoid decimation warnings
  rf_soapy_set_rx_srate(handler, 1.92e6);
  rf_soapy_set_tx_srate(handler, 1.92e6);

  // list device sensors
  size_t list_length;
  char** list;
  list = SoapySDRDevice_listSensors(handler->device, &list_length);
  printf("Available device sensors: \n");
  for (int i = 0; i < list_length; i++) {
    printf(" - %s\n", list[i]);
  }

  // list channel sensors
  for (uint32_t i = 0; i < handler->num_rx_channels; ++i) {
    list = SoapySDRDevice_listChannelSensors(handler->device, SOAPY_SDR_RX, i, &list_length);
    printf("Available sensors for Rx channel %d: \n", i);
    for (int j = 0; j < list_length; j++) {
      printf(" - %s\n", list[j]);
    }
  }

  // Set static radio info
  SoapySDRRange tx_range    = SoapySDRDevice_getGainRange(handler->device, SOAPY_SDR_TX, 0);
  SoapySDRRange rx_range    = SoapySDRDevice_getGainRange(handler->device, SOAPY_SDR_RX, 0);
  handler->info.min_tx_gain = tx_range.minimum;
  handler->info.max_tx_gain = tx_range.maximum;
  handler->info.min_rx_gain = rx_range.minimum;
  handler->info.max_rx_gain = rx_range.maximum;

  // Check device arguments
  if (args) {
    // config file
    const char config_arg[]   = "config=";
    char       config_str[64] = {0};
    char*      config_ptr     = strstr(args, config_arg);
    if (config_ptr) {
      copy_subdev_string(config_str, config_ptr + strlen(config_arg));
      printf("Loading config file %s\n", config_str);
      SoapySDRDevice_writeSetting(handler->device, "LOAD_CONFIG", config_str);
      remove_substring(args, config_arg);
      remove_substring(args, config_str);
    }

    // rx antenna
    const char rx_ant_arg[]   = "rxant=";
    char       rx_ant_str[64] = {0};
    char*      rx_ant_ptr     = strstr(args, rx_ant_arg);
    if (rx_ant_ptr) {
      copy_subdev_string(rx_ant_str, rx_ant_ptr + strlen(rx_ant_arg));
      for (int i = 0; i < handler->num_rx_channels; i++) {
        printf("Setting Rx channel %d antenna to %s\n", i, rx_ant_str);
        if (SoapySDRDevice_setAntenna(handler->device, SOAPY_SDR_RX, i, rx_ant_str) != 0) {
          ERROR("Failed to set Rx antenna for channel %d.", i);
        }
      }
      remove_substring(args, rx_ant_arg);
      remove_substring(args, rx_ant_str);
    }

    // tx antenna
    const char tx_ant_arg[]   = "txant=";
    char       tx_ant_str[64] = {0};
    char*      tx_ant_ptr     = strstr(args, tx_ant_arg);
    if (tx_ant_ptr) {
      copy_subdev_string(tx_ant_str, tx_ant_ptr + strlen(tx_ant_arg));
      for (int i = 0; i < handler->num_rx_channels; i++) {
        printf("Setting Tx channel %d antenna to %s\n", i, tx_ant_str);
        if (SoapySDRDevice_setAntenna(handler->device, SOAPY_SDR_TX, i, tx_ant_str) != 0) {
          ERROR("Failed to set Tx antenna for channel %d.", i);
        }
      }
      remove_substring(args, tx_ant_arg);
      remove_substring(args, tx_ant_str);
    }

    // log level
    const char loglevel_arg[]   = "loglevel=";
    char       loglevel_str[64] = {0};
    char*      loglevel_ptr     = strstr(args, loglevel_arg);
    if (loglevel_ptr) {
      copy_subdev_string(loglevel_str, loglevel_ptr + strlen(loglevel_arg));
      if (strcmp(loglevel_str, "error") == 0) {
        SoapySDR_setLogLevel(SOAPY_SDR_ERROR);
      }
      remove_substring(args, loglevel_arg);
      remove_substring(args, loglevel_str);
    }
  }

#if HAVE_ASYNC_THREAD
  bool start_async_thread = true;
  if (args) {
    if (strstr(args, "silent")) {
      REMOVE_SUBSTRING_WITHCOMAS(args, "silent");
      start_async_thread = false;
    }
  }
#endif

  // list gains and AGC mode
  bool has_agc = SoapySDRDevice_hasGainMode(handler->device, SOAPY_SDR_RX, 0);
  list         = SoapySDRDevice_listGains(handler->device, SOAPY_SDR_RX, 0, &list_length);
  printf("State of gain elements for Rx channel 0 (AGC %s):\n", has_agc ? "supported" : "not supported");
  for (int i = 0; i < list_length; i++) {
    printf(" - %s: %.2f dB\n", list[i], SoapySDRDevice_getGainElement(handler->device, SOAPY_SDR_RX, 0, list[i]));
  }

  has_agc = SoapySDRDevice_hasGainMode(handler->device, SOAPY_SDR_TX, 0);
  list    = SoapySDRDevice_listGains(handler->device, SOAPY_SDR_TX, 0, &list_length);
  printf("State of gain elements for Tx channel 0 (AGC %s):\n", has_agc ? "supported" : "not supported");
  for (int i = 0; i < list_length; i++) {
    printf(" - %s: %.2f dB\n", list[i], SoapySDRDevice_getGainElement(handler->device, SOAPY_SDR_TX, 0, list[i]));
  }

  // print actual antenna configuration
  char* ant = SoapySDRDevice_getAntenna(handler->device, SOAPY_SDR_RX, 0);
  printf("Rx antenna set to %s\n", ant);

  ant = SoapySDRDevice_getAntenna(handler->device, SOAPY_SDR_TX, 0);
  printf("Tx antenna set to %s\n", ant);

#if HAVE_ASYNC_THREAD
  if (start_async_thread) {
    // Start low priority thread to receive async commands
    handler->async_thread_running = true;
    if (pthread_create(&handler->async_thread, NULL, async_thread, handler)) {
      perror("pthread_create");
      return -1;
    }
  }
#endif

  return SRSRAN_SUCCESS;
}

int rf_soapy_open(char* args, void** h)
{
  return rf_soapy_open_multi(args, h, 1);
}

int rf_soapy_close(void* h)
{
  rf_soapy_handler_t* handler = (rf_soapy_handler_t*)h;

#if HAVE_ASYNC_THREAD
  if (handler->async_thread_running) {
    handler->async_thread_running = false;
    pthread_join(handler->async_thread, NULL);
  }
#endif

  if (handler->tx_stream_active) {
    rf_soapy_stop_tx_stream(handler);
    SoapySDRDevice_closeStream(handler->device, handler->txStream);
  }

  if (handler->rx_stream_active) {
    rf_soapy_stop_rx_stream(handler);
    SoapySDRDevice_closeStream(handler->device, handler->rxStream);
  }

  SoapySDRDevice_unmake(handler->device);

  // print statistics
  if (handler->num_lates)
    printf("#lates=%d\n", handler->num_lates);
  if (handler->num_overflows)
    printf("#overflows=%d\n", handler->num_overflows);
  if (handler->num_underflows)
    printf("#underflows=%d\n", handler->num_underflows);
  if (handler->num_time_errors)
    printf("#time_errors=%d\n", handler->num_time_errors);
  if (handler->num_other_errors)
    printf("#other_errors=%d\n", handler->num_other_errors);

  free(handler);

  return SRSRAN_SUCCESS;
}

double rf_soapy_set_rx_srate(void* h, double rate)
{
  rf_soapy_handler_t* handler = (rf_soapy_handler_t*)h;

#if STOP_STREAM_BEFORE_RATE_CHANGE
  // Restart streaming, as the Lime seems to have problems reconfiguring the sample rate during streaming
  bool rx_stream_active = handler->rx_stream_active;
  if (rx_stream_active) {
    rf_soapy_stop_rx_stream(handler);
  }
#endif // STOP_STREAM_BEFORE_RATE_CHANGE

  for (uint32_t i = 0; i < handler->num_rx_channels; i++) {
    if (SoapySDRDevice_setSampleRate(handler->device, SOAPY_SDR_RX, i, rate) != 0) {
      printf("setSampleRate Rx fail: %s\n", SoapySDRDevice_lastError());
      return SRSRAN_ERROR;
    }

#if SET_RF_BW
    // Set bandwidth close to current rate
    size_t         bw_length;
    SoapySDRRange* bw_range = SoapySDRDevice_getBandwidthRange(handler->device, SOAPY_SDR_RX, 0, &bw_length);
    for (int k = 0; k < bw_length; ++k) {
      double bw = rate * 0.75;
      bw        = SRSRAN_MIN(bw, bw_range[k].maximum);
      bw        = SRSRAN_MAX(bw, bw_range[k].minimum);
      if (SoapySDRDevice_setBandwidth(handler->device, SOAPY_SDR_RX, i, bw) != 0) {
        printf("setBandwidth fail: %s\n", SoapySDRDevice_lastError());
        return SRSRAN_ERROR;
      }
      printf("Set Rx bandwidth to %.2f MHz\n", SoapySDRDevice_getBandwidth(handler->device, SOAPY_SDR_RX, i) / 1e6);
    }
#endif // SET_RF_BW
  }

#if STOP_STREAM_BEFORE_RATE_CHANGE
  if (rx_stream_active) {
    rf_soapy_start_rx_stream(handler, true);
  }
#endif // STOP_STREAM_BEFORE_RATE_CHANGE

  // retrun sample rate of first channel
  return SoapySDRDevice_getSampleRate(handler->device, SOAPY_SDR_RX, 0);
}

double rf_soapy_set_tx_srate(void* h, double rate)
{
  rf_soapy_handler_t* handler = (rf_soapy_handler_t*)h;

#if STOP_STREAM_BEFORE_RATE_CHANGE
  // stop/start streaming during rate reconfiguration
  bool rx_stream_active = handler->rx_stream_active;
  if (handler->rx_stream_active) {
    rf_soapy_stop_rx_stream(handler);
  }
#endif // STOP_STREAM_BEFORE_RATE_CHANGE

  for (uint32_t i = 0; i < handler->num_tx_channels; i++) {
    if (SoapySDRDevice_setSampleRate(handler->device, SOAPY_SDR_TX, i, rate) != 0) {
      printf("setSampleRate Tx fail: %s\n", SoapySDRDevice_lastError());
      return SRSRAN_ERROR;
    }

#if SET_RF_BW
    size_t         bw_length;
    SoapySDRRange* bw_range = SoapySDRDevice_getBandwidthRange(handler->device, SOAPY_SDR_TX, i, &bw_length);
    for (int k = 0; k < bw_length; ++k) {
      // try to set the BW a bit narrower than sampling rate to prevent aliasing but make sure to stay within device
      // boundaries
      double bw = rate * 0.75;
      bw        = SRSRAN_MAX(bw, bw_range[k].minimum);
      bw        = SRSRAN_MIN(bw, bw_range[k].maximum);
      if (SoapySDRDevice_setBandwidth(handler->device, SOAPY_SDR_TX, i, bw) != 0) {
        printf("setBandwidth fail: %s\n", SoapySDRDevice_lastError());
        return SRSRAN_ERROR;
      }
      printf("Set Tx bandwidth to %.2f MHz\n", SoapySDRDevice_getBandwidth(handler->device, SOAPY_SDR_TX, i) / 1e6);
    }
#endif // SET_RF_BW
  }

#if STOP_STREAM_BEFORE_RATE_CHANGE
  if (rx_stream_active) {
    rf_soapy_start_rx_stream(handler, true);
  }
#endif // STOP_STREAM_BEFORE_RATE_CHANGE

  handler->tx_rate = SoapySDRDevice_getSampleRate(handler->device, SOAPY_SDR_TX, 0);

  return handler->tx_rate;
}

int rf_soapy_set_rx_gain(void* h, double gain)
{
  rf_soapy_handler_t* handler = (rf_soapy_handler_t*)h;
  for (uint32_t i = 0; i < handler->num_rx_channels; i++) {
    if (rf_soapy_set_rx_gain_ch(h, i, gain) < 0) {
      return SRSRAN_ERROR;
    }
  }
  return SRSRAN_SUCCESS;
}

int rf_soapy_set_rx_gain_ch(void* h, uint32_t ch, double gain)
{
  rf_soapy_handler_t* handler = (rf_soapy_handler_t*)h;
  if (SoapySDRDevice_setGain(handler->device, SOAPY_SDR_RX, ch, gain) != 0) {
    printf("setGain fail: %s\n", SoapySDRDevice_lastError());
    return SRSRAN_ERROR;
  }
  return SRSRAN_SUCCESS;
}

int rf_soapy_set_tx_gain(void* h, double gain)
{
  rf_soapy_handler_t* handler = (rf_soapy_handler_t*)h;
  for (uint32_t i = 0; i < handler->num_tx_channels; i++) {
    if (rf_soapy_set_tx_gain_ch(h, i, gain) < 0) {
      return SRSRAN_ERROR;
    }
  }
  return SRSRAN_SUCCESS;
}

int rf_soapy_set_tx_gain_ch(void* h, uint32_t ch, double gain)
{
  rf_soapy_handler_t* handler = (rf_soapy_handler_t*)h;
  if (SoapySDRDevice_setGain(handler->device, SOAPY_SDR_TX, ch, gain) != 0) {
    printf("setGain fail: %s\n", SoapySDRDevice_lastError());
    return SRSRAN_ERROR;
  }
  return SRSRAN_SUCCESS;
}

// Return gain of first channel
double rf_soapy_get_rx_gain(void* h)
{
  rf_soapy_handler_t* handler = (rf_soapy_handler_t*)h;
  return SoapySDRDevice_getGain(handler->device, SOAPY_SDR_RX, 0);
}

// Return gain of first channel
double rf_soapy_get_tx_gain(void* h)
{
  rf_soapy_handler_t* handler = (rf_soapy_handler_t*)h;
  return SoapySDRDevice_getGain(handler->device, SOAPY_SDR_TX, 0);
}

srsran_rf_info_t* rf_soapy_get_info(void* h)
{
  srsran_rf_info_t* info = NULL;
  if (h) {
    rf_soapy_handler_t* handler = (rf_soapy_handler_t*)h;
    info                        = &handler->info;
  }
  return info;
}

double rf_soapy_set_rx_freq(void* h, uint32_t ch, double freq)
{
  rf_soapy_handler_t* handler = (rf_soapy_handler_t*)h;

  for (uint32_t i = 0; i < handler->num_rx_channels; i++) {
    if (SoapySDRDevice_setFrequency(handler->device, SOAPY_SDR_RX, i, freq, NULL) != 0) {
      printf("setFrequency fail: %s\n", SoapySDRDevice_lastError());
      return SRSRAN_ERROR;
    }
  }

  // wait until LO is locked
  rf_soapy_rx_wait_lo_locked(handler);

  // Return actual frequency for channel 0
  return SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_RX, 0);
}

double rf_soapy_set_tx_freq(void* h, uint32_t ch, double freq)
{
  rf_soapy_handler_t* handler = (rf_soapy_handler_t*)h;
  for (uint32_t i = 0; i < handler->num_tx_channels; i++) {
    if (SoapySDRDevice_setFrequency(handler->device, SOAPY_SDR_TX, i, freq, NULL) != 0) {
      printf("setFrequency fail: %s\n", SoapySDRDevice_lastError());
      return SRSRAN_ERROR;
    }
  }
  return SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_TX, 0);
}

void rf_soapy_get_time(void* h, time_t* secs, double* frac_secs)
{
  printf("Todo: implement rf_soapy_get_time()\n");
}

// TODO: add multi-channel support
int rf_soapy_recv_with_time_multi(void*    h,
                                  void*    data[SRSRAN_MAX_PORTS],
                                  uint32_t nsamples,
                                  bool     blocking,
                                  time_t*  secs,
                                  double*  frac_secs)
{
  rf_soapy_handler_t* handler   = (rf_soapy_handler_t*)h;
  int                 flags     = 0;      // flags set by receive operation
  const long          timeoutUs = 400000; // arbitrarily chosen

  int       trials = 0;
  int       ret    = 0;
  long long timeNs; // timestamp for receive buffer
  int       n = 0;

#if PRINT_RX_STATS
  printf("rx: nsamples=%d rx_mtu=%zd\n", nsamples, handler->rx_mtu);
#endif

  do {
    size_t rx_samples = SRSRAN_MIN(nsamples - n, handler->rx_mtu);
#if PRINT_RX_STATS
    printf(" - rx_samples=%zd\n", rx_samples);
#endif

    void* buffs_ptr[SRSRAN_MAX_PORTS] = {};
    for (int i = 0; i < handler->num_rx_channels; i++) {
      cf_t* data_c = (cf_t*)data[i];
      buffs_ptr[i] = &data_c[n];
    }

    ret = SoapySDRDevice_readStream(
        handler->device, handler->rxStream, buffs_ptr, rx_samples, &flags, &timeNs, timeoutUs);
    if (ret == SOAPY_SDR_OVERFLOW || (ret > 0 && (flags & SOAPY_SDR_END_ABRUPT) != 0)) {
      log_overflow(handler);
      continue;
    } else if (ret == SOAPY_SDR_TIMEOUT) {
      log_late(handler, true);
      continue;
    } else if (ret < 0) {
      // unspecific error
      printf("SoapySDRDevice_readStream returned %d: %s\n", ret, SoapySDR_errToStr(ret));
      handler->num_other_errors++;
    }

    // update rx time only for first segment
    if (secs != NULL && frac_secs != NULL && n == 0) {
      *secs      = floor(timeNs / 1e9);
      *frac_secs = (timeNs % 1000000000) / 1e9;
      // printf("rx_time: secs=%lld, frac_secs=%lf timeNs=%llu\n", *secs, *frac_secs, timeNs);
    }

#if PRINT_RX_STATS
    printf(" - rx: %d/%zd\n", ret, rx_samples);
#endif

    n += ret;
    trials++;
  } while (n < nsamples && trials < 100);

  return n;
}

int rf_soapy_recv_with_time(void* h, void* data, uint32_t nsamples, bool blocking, time_t* secs, double* frac_secs)
{
  void* data_multi[SRSRAN_MAX_PORTS] = {NULL};
  data_multi[0]                      = data;
  return rf_soapy_recv_with_time_multi(h, data_multi, nsamples, blocking, secs, frac_secs);
}

int rf_soapy_send_timed(void*  h,
                        void*  data,
                        int    nsamples,
                        time_t secs,
                        double frac_secs,
                        bool   has_time_spec,
                        bool   blocking,
                        bool   is_start_of_burst,
                        bool   is_end_of_burst)
{
  void* _data[SRSRAN_MAX_PORTS] = {data, zero_mem, zero_mem, zero_mem};
  return rf_soapy_send_timed_multi(
      h, _data, nsamples, secs, frac_secs, has_time_spec, blocking, is_start_of_burst, is_end_of_burst);
}

// Todo: Check correct handling of flags, use RF metrics API, fix timed transmissions
int rf_soapy_send_timed_multi(void*  h,
                              void*  data[SRSRAN_MAX_PORTS],
                              int    nsamples,
                              time_t secs,
                              double frac_secs,
                              bool   has_time_spec,
                              bool   blocking,
                              bool   is_start_of_burst,
                              bool   is_end_of_burst)
{
  rf_soapy_handler_t* handler   = (rf_soapy_handler_t*)h;
  int                 flags     = 0;
  const long          timeoutUs = 100000; // arbitrarily chosen
  long long           timeNs    = 0;
  int                 trials    = 0;
  int                 ret       = 0;
  int                 n         = 0;

#if PRINT_TX_STATS
  printf("tx: namples=%d, mtu=%zd\n", nsamples, handler->tx_mtu);
#endif

  if (!handler->tx_stream_active) {
    rf_soapy_start_tx_stream(h);
  }

  // Convert initial tx time
  if (has_time_spec) {
    timeNs = (long long)secs * 1000000000;
    timeNs = timeNs + (frac_secs * 1000000000);
  }

  do {
#if USE_TX_MTU
    size_t tx_samples = SRSRAN_MIN(nsamples - n, handler->tx_mtu);
#else
    size_t tx_samples = nsamples;
    if (tx_samples > nsamples - n) {
      tx_samples = nsamples - n;
    }
#endif

    // (re-)set stream flags
    flags = 0;
    if (is_start_of_burst && is_end_of_burst) {
      flags |= SOAPY_SDR_ONE_PACKET;
    }

    if (is_end_of_burst) {
      flags |= SOAPY_SDR_END_BURST;
    }

    // only set time flag for first tx
    if (has_time_spec && n == 0) {
      flags |= SOAPY_SDR_HAS_TIME;
    }

#if PRINT_TX_STATS
    printf(" - tx_samples=%zd at timeNs=%llu flags=%d\n", tx_samples, timeNs, flags);
#endif

    const void* buffs_ptr[SRSRAN_MAX_PORTS] = {};
    for (int i = 0; i < handler->num_tx_channels; i++) {
      cf_t* data_c = data[i] ? data[i] : zero_mem;
      buffs_ptr[i] = &data_c[n];
    }

    ret = SoapySDRDevice_writeStream(
        handler->device, handler->txStream, buffs_ptr, tx_samples, &flags, timeNs, timeoutUs);
    if (ret >= 0) {
      // Tx was ok
#if PRINT_TX_STATS
      printf(" - tx: %d/%zd\n", ret, tx_samples);
#endif
      // Advance tx time
      if (has_time_spec && ret < nsamples) {
        long long adv = SoapySDR_ticksToTimeNs(ret, handler->tx_rate);
#if PRINT_TX_STATS
        printf(" - tx: timeNs_old=%llu, adv=%llu, timeNs_new=%llu, tx_rate=%f\n",
               timeNs,
               adv,
               timeNs + adv,
               handler->tx_rate);
#endif
        timeNs += adv;
      }
      n += ret;
    } else if (ret < 0) {
      // An error has occured
      switch (ret) {
        case SOAPY_SDR_TIMEOUT:
          log_late(handler, false);
          printf("L");
          break;
        case SOAPY_SDR_STREAM_ERROR:
          handler->num_stream_curruption++;
          printf("E");
          break;
        case SOAPY_SDR_TIME_ERROR:
          handler->num_time_errors++;
          printf("T");
          break;
        case SOAPY_SDR_UNDERFLOW:
          log_underflow(handler);
          printf("U");
          break;
        default:
          ERROR("Error during writeStream");
          exit(-1);
          return SRSRAN_ERROR;
      }
    }
    trials++;
  } while (n < nsamples && trials < 100);

  if (n != nsamples) {
    ERROR("Couldn't write all samples after %d trials.", trials);
  }

  return n;
}

rf_dev_t srsran_rf_dev_soapy = {"soapy",
                                rf_soapy_devname,
                                rf_soapy_start_rx_stream,
                                rf_soapy_stop_rx_stream,
                                rf_soapy_flush_buffer,
                                rf_soapy_has_rssi,
                                rf_soapy_get_rssi,
                                rf_soapy_suppress_stdout,
                                rf_soapy_register_error_handler,
                                rf_soapy_open,
                                rf_soapy_open_multi,
                                rf_soapy_close,
                                rf_soapy_set_rx_srate,
                                rf_soapy_set_rx_gain,
                                rf_soapy_set_rx_gain_ch,
                                rf_soapy_set_tx_gain,
                                rf_soapy_set_tx_gain_ch,
                                rf_soapy_get_rx_gain,
                                rf_soapy_get_tx_gain,
                                rf_soapy_get_info,
                                rf_soapy_set_rx_freq,
                                rf_soapy_set_tx_srate,
                                rf_soapy_set_tx_freq,
                                rf_soapy_get_time,
                                NULL,
                                rf_soapy_recv_with_time,
                                rf_soapy_recv_with_time_multi,
                                rf_soapy_send_timed,
                                .srsran_rf_send_timed_multi = rf_soapy_send_timed_multi};

#ifdef ENABLE_RF_PLUGINS
int register_plugin(rf_dev_t** rf_api)
{
  if (rf_api == NULL) {
    return SRSRAN_ERROR;
  }
  *rf_api = &srsran_rf_dev_soapy;
  return SRSRAN_SUCCESS;
}
#endif /* ENABLE_RF_PLUGINS */
