/* -*- c++ -*- */
/*
 * Copyright 2003,2006,2008 Free Software Foundation, Inc.
 * 
 * This file is part of GNU Radio
 * 
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 * 
 * GNU Radio 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 General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#include "usrp_tree_config.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <usb.h>			/* needed for usb functions */
#include <getopt.h>
#include <assert.h>
#include <math.h>
#include <iostream>
#include "usrp_standard.h"
#include "usrp_bytesex.h"
#include "fpga_regs_common.h"
#include "fpga_regs_standard.h"

#ifdef HAVE_SCHED_H
#include <sched.h>
#endif

using std::cerr;
using std::endl;

char *prog_name;

static bool test_input  (usrp_standard_rx_sptr urx, int max_bytes, FILE *fp);

static void
set_progname (char *path)
{
  char *p = strrchr (path, '/');
  if (p != 0)
    prog_name = p+1;
  else
    prog_name = path;
}

static void
usage ()
{
  fprintf (stderr, "usage: %s [-f] [-v] [-l] [-c] [-D <decim>] [-F freq] [-o output_file]\n", prog_name);
  fprintf (stderr, "  [-f] loop forever\n");
  fprintf (stderr, "  [-M] how many Megabytes to transfer (default 128)\n");
  fprintf (stderr, "  [-v] verbose\n");
  fprintf (stderr, "  [-l] digital loopback in FPGA\n");
  fprintf (stderr, "  [-c] counting in FPGA\n");
  fprintf (stderr, "  [-p] output DDC phase\n");
  fprintf (stderr, "  [-S] use external synchronization\n");
  //  fprintf (stderr, "  [-8] 8-bit samples across USB\n");
  fprintf (stderr, "  [-B <fusb_block_size>] set fast usb block_size\n");
  fprintf (stderr, "  [-N <fusb_nblocks>] set fast usb nblocks\n");
#ifdef HAVE_SCHED_H
  fprintf (stderr, "  [-R] set real time scheduling: SCHED_FIFO; pri = midpoint\n");
#endif

  exit (1);
}

static void
die (const char *msg)
{
  fprintf (stderr, "die: %s: %s\n", prog_name, msg);
  exit (1);
}

void show_db_info(usrp_standard_rx_sptr urx, int which)
{
  cerr << "Daughterboard " << which << ":\n";

  db_base_sptr db = urx->db()[which][0];
  if (!db) {
    cerr << "  None found\n";
    return;
  }

  cerr << "  Daughterboard is a " << db->name() << endl;
  cerr << "  Gain range is " << db->gain_min() << " - " << db->gain_max()
       << " with " << db->gain_db_per_step() << "dB per step.\n";
  fprintf(stderr, "  Freq range is %.2f - %.2f MHz.\n",
	  db->freq_min() * 1e-6, db->freq_max() * 1e-6);
  cerr << "  Device is " << (db->is_quadrature() ? "" : "not ")
       << "quadrature.\n";
  cerr << "  Spectrum is " << (db->spectrum_inverted() ? "" : "not ")
       << "inverted.\n";
}

void tune_one_channel(usrp_standard_rx_sptr urx, int which, double center_freq)
{
  db_base_sptr db = urx->db()[which][0];

  // The "obvious" way is db->set_freq(center_freq),
  // but that ends up with a huge offset because the baseband
  // is probably not DC.  So we use the magic helper, which does
  // further processing in the FPGA.
  // A future incarnation might want to skip the latter
  // step and do it in software for added control, which is problematic
  // due to bandwidth issues.

  usrp_tune_result result;
  if (!urx->tune(which, db, center_freq, &result))
    die("Tuning failed");

  fprintf(stderr, "Tune request for channel %d to %.6lfMHz:\n",
	  which, center_freq * 1e-6);
  fprintf(stderr, "  baseband=%.6lfMHz, dxc=%.6fMHz\n",
	  result.baseband_freq * 1e-6, result.dxc_freq * 1e-6);
  fprintf(stderr, "  residual error=%.4fHz\n", result.residual_freq);
  fprintf(stderr, "  %sinverted\n", result.inverted ? "" : "not ");
}

void tune_both_channels(usrp_standard_rx_sptr urx, double center_freq)
{
  if (!urx->set_rx_freq (0, center_freq))
    die ("urx->set_rx_freq");

  tune_one_channel(urx, 0, center_freq);
  tune_one_channel(urx, 1, center_freq);
}

int
main (int argc, char **argv)
{
  bool 	verbose_p = false;
  bool	loopback_p = false;
  bool  counting_p = false;
  bool  phase_p = false;
  bool  sync_p = false;
  //  bool	width_8_p = false;
  int   max_bytes = 128 * (1L << 20);
  int	ch;
  char	*output_filename = 0;
  int	decim = 16;			// 32 MB/sec
  double	center_freq = 0;
  float gain = 0;
  int	fusb_block_size = 0;
  int	fusb_nblocks = 0;
  bool	realtime_p = false;


  set_progname (argv[0]);

  while ((ch = getopt (argc, argv, "fvlcpSo:g:D:F:M:B:N:R")) != EOF){
    switch (ch){
    case 'f':
      max_bytes = 0;
      break;

    case 'v':
      verbose_p = true;
      break;

    case 'l':
      loopback_p = true;
      break;

    case 'c':
      counting_p = true;
      break;

    case 'p':
      phase_p = true;
      break;

    case 'S':
      sync_p = true;
      break;
    
      /*  
    case '8':
      width_8_p = true;
      break;
      */
      
    case 'o':
      output_filename = optarg;
      break;
      
    case 'D':
      decim = strtol (optarg, 0, 0);
      break;

    case 'F':
      center_freq = strtod (optarg, 0);
      break;

    case 'g':
      gain = strtof (optarg, 0);
      break;

    case 'M':
      max_bytes = strtol (optarg, 0, 0) * (1L << 20);
      if (max_bytes < 0) max_bytes = 0;
      break;

    case 'B':
      fusb_block_size = strtol (optarg, 0, 0);
      break;

    case 'N':
      fusb_nblocks = strtol (optarg, 0, 0);
      break;

    case 'R':
      realtime_p = true;
      break;

    default:
      usage ();
    }
  }

#ifdef HAVE_SCHED_SETSCHEDULER
  if (realtime_p){
    int policy = SCHED_FIFO;
    int pri = (sched_get_priority_max (policy) - sched_get_priority_min (policy)) / 2;
    int pid = 0;  // this process

    struct sched_param param;
    memset(&param, 0, sizeof(param));
    param.sched_priority = pri;
    int result = sched_setscheduler(pid, policy, &param);
    if (result != 0){
      perror ("sched_setscheduler: failed to set real time priority");
    }
    else
      printf("SCHED_FIFO enabled with priority = %d\n", pri);
  }
#endif

  FILE *fp = 0;

  if (output_filename){
    fp = fopen (output_filename, "wb");
    if (fp == 0)
      perror (output_filename);
  }
      
  int mode = 0;
  if (loopback_p)
    mode |= usrp_standard_rx::FPGA_MODE_LOOPBACK;
  if (counting_p)
    mode |= usrp_standard_rx::FPGA_MODE_COUNTING;
  if (phase_p)
    mode |= 0x4;
  if (sync_p)
    mode |= 0x8;


  usrp_standard_rx_sptr urx = 
    usrp_standard_rx::make (0, decim, 2, -1, mode,
			    fusb_block_size, fusb_nblocks);

  if (!urx)
    die ("usrp_standard_rx::make");

  // Enumerate the daughterboards (sorry no BasicRX supported yet)
  if (verbose_p) {
    cerr << "Connected to USRP with s/n " << urx->serial_number() << endl;
    cerr << "Decimation is " << decim << endl << endl;
    show_db_info(urx, 0);
    show_db_info(urx, 1);
    cerr << endl;
  }

  if (center_freq == 0) {
    cerr << "WARNING: No center freq specified (mystery data)!\n";
  } else {
    tune_both_channels(urx, center_freq);
  }

  // Note: if we hacked around a bit, we might get
  // *separate* RF, IF, and AGC gain controls.  This
  // may be tweakable to get better performance.
  if (!urx->db()[0][0]->set_gain(gain))
    die("Failed to set channel 0 gain");
  if (!urx->db()[1][0]->set_gain(gain))
    die("Failed to set channel 1 gain");
  if (verbose_p)
    cerr << "Gain set on both channels to " << gain << endl;
    
  /*
  if (width_8_p){
    int width = 8;
    int shift = 8;
    bool want_q = true;
    if (!urx->set_format(usrp_standard_rx::make_format(width, shift, want_q)))
      die("urx->set_format");
  }
  */

  {
    timeval tv;
    tv.tv_sec = 2;
    tv.tv_usec = 0;
    if (verbose_p)
      fprintf(stderr, "Waiting 2 seconds for oscillators to settle... ");
    select(0, 0, 0, 0, &tv);
    if (verbose_p)
      fprintf(stderr, "done\n");
  }

  if (verbose_p && 0) {
    // Oddly this screws everything up.  So it's disabled.
    int mux_val;
    if (!urx->_read_fpga_reg(FR_RX_MUX, &mux_val))
      die("Failed to read HW mux register");
    fprintf(stderr, "HW mux value = 0x%08X\n", (unsigned int)mux_val);
  }

  urx->start();		// start data xfers

  test_input (urx, max_bytes, fp);

  if (fp)
    fclose (fp);

  return 0;
}

struct timespec read_clock(clockid_t clk_id)
{
  timespec ret;
  if (clock_gettime(clk_id, &ret) != 0)
    die("clock_gettime");
  return ret;
}

int64_t operator - (const timespec &end, const timespec &start)
{
  return (end.tv_nsec - start.tv_nsec)
    + (end.tv_sec - start.tv_sec) * 1000000000LL;
}

static bool
test_input  (usrp_standard_rx_sptr urx, int max_bytes, FILE *fp)
{
  int		   fd = -1;
  static const int BUFSIZE = urx->block_size();
  static const int N = BUFSIZE/sizeof (short);
  short 	   buf[N];
  int		   nbytes = 0;

  timespec start_monotonic = read_clock(CLOCK_MONOTONIC);
  timespec start_cpu = read_clock(CLOCK_THREAD_CPUTIME_ID);

  if (fp)
    fd = fileno (fp);
  
  bool overrun;
  int noverruns = 0;

  double sumsquares = 0.0;
  uint64_t n = 0;

  for (nbytes = 0; max_bytes == 0 || nbytes < max_bytes; nbytes += BUFSIZE){

    unsigned int	ret = urx->read (buf, sizeof (buf), &overrun);
    if (ret != sizeof (buf)){
      fprintf (stderr, "test_input: error, ret = %d\n", ret);
    }

    if (overrun){
      printf ("rx_overrun\n");
      noverruns++;
    }
    
    double localsumsquares = 0.0;
    for (unsigned int i = 0; i < sizeof (buf) / sizeof (short); i++) {
      buf[i] = usrp_to_host_short (buf[i]);
      localsumsquares += buf[i] * buf[i];
    }

    sumsquares += localsumsquares;
    n += sizeof(buf) / sizeof(short);

    if (fd != -1){
      if (write (fd, buf, sizeof (buf)) == -1){
	perror ("write");
	fd = -1;
      }
    }
  }

  timespec stop_cpu = read_clock(CLOCK_THREAD_CPUTIME_ID);
  timespec stop_monotonic = read_clock(CLOCK_MONOTONIC);

  double delta_wall = (stop_monotonic - start_monotonic) * 1e-9;
  double delta_cpu  = (stop_cpu - start_cpu) * 1e-9;

  printf ("xfered %.2f MB in %.3f seconds.  %.2f MB/sec.  cpu time = %.4f\n",
	  max_bytes * 1e-6, delta_wall,
	  max_bytes / delta_wall * 1e-6, delta_cpu);
  printf ("noverruns = %d\n", noverruns);

  if (n) {
    double meansquare = sumsquares / n;
    printf ("mean power = %.2fdBfs\n",
	    10*log10(meansquare / ((1<<14) * (1<<14))));
  }

  return true;
}