path: root/src/lockin.cxx

// https://doc.qt.io/qt-6/qtexamplesandtutorials.html
// especially:
// https://doc.qt.io/qt-6/qtcharts-qmloscilloscope-example.html
// code:
// https://code.qt.io/qt/qtcharts.git
// https://code.qt.io/cgit/qt/qtcharts.git/tree/examples/charts/qmloscilloscope?h=6.6
// efficiency:
// https://stackoverflow.com/questions/54993973/efficient-curve-plotting-using-qwtplotcurve

///////////////////////////////////////////////////////////////////
// software lockin
// i.e. software lock-in amplifier
// i.e. software synchronous detector
// i.e. software synchronous analyzer
// i.e. software synchronous wave analyzer

using namespace std;
#include <iostream>
#include <iomanip>
#include <stdlib.h>
#include <stdio.h>
#include "Getopt.h"
#include <unistd.h>
#include <signal.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/ioctl.h>
#include <sys/wait.h>
#include <asm/types.h>
#include <time.h>
#include <sys/time.h>
#include <errno.h>
#include <string>
#include <vector>
#include <valarray>

// On debian, you need to install the development package,
//  i.e. libasound2-dev (not just libasound2) to provide
//  alsa/asoundlib.h and the associated runtime libraries:
#include <alsa/asoundlib.h>

#include <qwt_plot.h>
#include <qwt_plot_curve.h>
#include <qwt_plot_grid.h>

#include "lockin.h"

#include "gui.h"
#include "gui_class.h"
#include "refout.h"
#include "lockin.h"
#include "krunch.h"

#ifdef FOOBAR
#include "alsactl.h"
#else                             /* backwards compatibility */
//#  warning You may want to use ./excl.patch to implement ALSA exclusive access.
#  define SND_CTL_RDONLY 0
#endif

// a few non-constant global variables:
int verbosity=0;
int xrun_verbosity(1);           // be verbose about underrun and overrun
string progname;
timespec prog_start_time;

// Forward references:

void config_mixer(const string ctlfile, const string justcard);
void discard(int signum);
void trap_exit(int signum);
void trap_detach(int signum);
void drain(const char* draindev);
void drain(const int fd);
int isfile(const char* fname);

// Functions

string fixup_ctlfile(string ctlfile, const string justcard, const int mode=0);

void snark::usage(const int err) {
  (err ? cerr : cout) <<
"Harvest entropy from the audio I/O system.\n"
"Usage: " << progname << " [options]\n\n"
"Options:\n"
"--buffer-size, -b []  Number of frames to read at a time. [= "
        << nframe << "].\n"
"--card,        -c []  Sound card, subdevice to use. [= "
        << card_device << "].\n"
"               -c -1  List available capture cards.\n"
"               -c -2  List available playback cards.\n"
"--mixer-ctl,   -m []  Mixer set-up from alsactl file. "
        "[= " << fixup_ctlfile(mixer_ctlfile, justcard) << "];\n"
"               -m \"\" ==> don't set up; just use inherited settings.\n"
"--Amplitude,   -A []  RefOut amplitude / dBV "
                        "[= " << refOutAmp_dB << "]\n"
"--Channel-mask -C []  1==>left 2==>right 3==> stereo etc.\n"
"--Frequency,   -F []  RefOut frequency / Hz (useful= 440).\n"
"--Delta-f,     -D []  RefOut 2nd chan freq relative to 1st"
                        " [= " << refival << "].\n"
"--Vo-cal,      -O []  Output voltage calibration / dBV full scale [0]\n"
"--Vi-cal,      -I []  Input  voltage calibration / dBV full scale [0]\n"
"                      (-45 dbV FS is plausible for mic input)\n"
"--Time-shift   -T []  Advance reference by ... sec [0].\n"
"--Phase-shift, -P []  Advance reference by ... degrees [0].\n"
"--frame-rate,  -N []  Audio measurement rate, frames per second;\n"
"               -N 0 ==> default ==> max rate supported by the hardware.\n"
"               -N -1 ==> probe for capture capabilities.\n"
"               -N -2 ==> probe for output capabilities.\n"
"--Zref,        -Z []  External reference resistor / Ohms "
                        "[= " << zref << "]\n"
"--verbose,     -v     Print extra debugging info (-vv ==> even more).\n"
"--write-check, -w []  Write checkfile containing one buffer of audio.\n"
"--1pass,       -1     Exit after one pass through main loop.\n"
<< endl;
}

// mode 0 is normal
string fixup_ctlfile(string ctlfile, const string justcard, const int mode){
  if (ctlfile == "<>") {
    ctlfile = mode==0 ? invent_ctlfile(justcard)
                : invent_driverfile(justcard);
  }
  // else cmdline has explict ctlfile, use that

// prepend explicit path, if needed:

  if (ctlfile.length()) {
    char mx = ctlfile[0];
    if (mx != '.' && mx != '/') {
      ctlfile = "/etc/" + progname + "/" + ctlfile;
      const char* homish = getenv("HOME");
      if (!homish) homish = "/root";
      string home(homish);
      if (home != "/root") {
        ctlfile = home + ctlfile;
      }
    } // else . or / means use it verbatim
  }
  return ctlfile;
}

// Just like snd_pcm_writei(),
// except the buff is always of the type ofdatum (int32_t),
// no matter what type the raw hardware uses.
// Return value:
//   nonnegative: # of frames written
//   negative: error code
int writei(const alsa_pcm* pcm, const datum* buff, const int nframe){
  if (pcm->format == SND_PCM_FORMAT_S32_LE) {
    return snd_pcm_writei(pcm->phndl, buff, nframe);
  } else if (pcm->format == SND_PCM_FORMAT_S16_LE) {
    valarray<int16_t> tmp(nframe * pcm->nchan);
    int16_t* to (&tmp[0]);
    const datum* from (buff);
    const int fudge(1<<16);
    for (unsigned int ii = 0; ii < nframe * pcm->nchan; ii++){
      *to++ = *from++ / fudge;
    }
    return snd_pcm_writei(pcm->phndl, &tmp[0], nframe);
  } else if (pcm->format == SND_PCM_FORMAT_S8) {
    valarray<char> tmp(nframe * pcm->nchan);
    char* to (&tmp[0]);
    const datum* from (buff);
    const int fudge(1<<24);
    for (unsigned int ii = 0; ii < nframe * pcm->nchan; ii++){
      *to++ = *from++ / fudge;
    }
    return snd_pcm_writei(pcm->phndl, &tmp[0], nframe);
  } else {
    cerr << "Don't know how to convert pcm data to format "
          <<   snd_pcm_format_name(pcm->format) << endl;
    exeunt(1);
  }
  return 0;             // defeat stupid compiler warning
}

// Just like the library routine of the same name, except:
// 1) The first arg is a "keeper",
//   which means that we will accept writes that are
//   not an integer multiple of the period.
// 2) The second arg is a datum* (not void*).
//   We call writei() to convert things to the sample-size
//   of the actual device.
snd_pcm_sframes_t snd_pcm_writei(Keeper& kk, const datum* buffer,
        snd_pcm_uframes_t nframes){
  int rslt;
  int todo(nframes);
  int head(0);          // frames taken from head of buffer
  const datum* mybuf(buffer);
  datum* start = &kk.buf[0];
  if (kk.finbuf) {
    head = kk.fpp - kk.finbuf;
    memcpy(start + kk.nchan*kk.finbuf, buffer, kk.nchan*sizeof(datum)*head);
    rslt = writei(kk.pcm, start, kk.fpp);
    if (rslt <= 0) return rslt;         // no change in kk
    if (rslt != kk.fpp) return -98;
    todo -= head;
    kk.finbuf = 0;
  }
  mybuf += kk.nchan*head;
  rslt = writei(kk.pcm, mybuf, todo);
  if (rslt < 0) return rslt;    // some error, let caller deal with it
  if (rslt == 0) return head;           // head got written, nothing more
// OK, at this point rslt must be positive.
  int left(todo - rslt);                // number of frames left to do....
  if (!left) return nframes;            // Nothing?  Great!
  if (left < kk.fpp) {          // can we keep it for next time?
    kk.finbuf = left;
    memcpy(start, mybuf + kk.nchan*rslt, kk.nchan*sizeof(datum)*left);
    return nframes;
  }
// here with a short write that doesn't fit in our keep-buffer:
  kk.finbuf = 0;
  return head + rslt;
}

// Constructor
Keeper::Keeper(const alsa_pcm* _dsp, const int _fpp)
  : pcm(_dsp),                  // remember the args
    fpp(_fpp),
    buf(pcm->nchan * _fpp),     // allocate the buffer
    finbuf(0),
    nchan(_dsp->nchan)
{} // no code, just initializers (above)


// Constructor
snark::snark() :
  card_device("PCH"),
  wcheck_fname(""),
  nframe(2048),
  onepass(0),
  desired_rate(0),
  channel_mask(-1),
  kout(0),
  zref(1e6),                    // assume reference = 1 megohm
  reffreq(440.),                // default : concert A440
  refival(440.5/440.),
  mixer_ctlfile(""),
  wcheck_fd(-1),
  sps(0),
  snooze(.5)
{}      // no code, just the initializers (above)


string crtRed   ("\033[1;31m");
string crtNormal("\033[0;39m");

vector<int> clean_me;

void trapme(){
  signal(SIGHUP, trap_exit);            // hangup
  signal(SIGINT, trap_exit);            // interrupt
  signal(SIGTERM, trap_exit);           // terminate
  signal(SIGPIPE, trap_exit);           // broken pipe
  signal(SIGCHLD, SIG_DFL);
}


double rms(const int* ptr, const int nn){
  if (!nn) return 0;
  double val;
  double sum(0);
  for (int ii = 0; ii < nn; ii++){
    val = ptr[ii];
    sum += val*val;
  }
  sum /= nn;
  return sqrt(sum);
}

int main(int argc, char** argv) {

// first, initialize the gui, so it can calculate
// some things (e.g. actOutFreq) that other tasks
// (e.g. refout) will need.
  int xargc(1);
  char tmpfoo[5];
  strncpy(tmpfoo, "foo", sizeof tmpfoo);
  char * xargv[xargc] = {tmpfoo};
  QApplication app(xargc, xargv);
  app.setStyle("plastique");

  progname = argv[0];
  string::size_type where = progname.rfind('/');        // find final slash
  if (where != progname.npos) {
    progname = progname.substr(1+where);
  }
  snark foo;                            // holder for cmdline options ...
  foo.cmdline(argc, argv);              // ... process them
  trapme();                             // install trap handlers

// be sure to setup the capture handler first, so
// on a soundblaster16 capture gets the 16-bit DMA
// channel and refout gets the 8-bit channel
  alsa_pcm in("capture");
  int err;
  if (foo.desired_rate >= -1) {
    err = in.getrate("hw:" + foo.card_device,
          SND_PCM_STREAM_CAPTURE, foo.desired_rate);
    if (err) exit(1);           // msgs have already been printed
  }

  alsa_pcm out("refout");
  if (foo.reffreq                       // off ==> don't even initialize
      || foo.desired_rate < 0) {        // unless we are just dumping params
    err = out.getrate("hw:" + foo.card_device,
          SND_PCM_STREAM_PLAYBACK, foo.desired_rate);
    if (err) exit(1);           // msgs have already been printed
  }

  int rate = min(in.max_rate, out.max_rate);
  if (foo.desired_rate) rate = min(rate, foo.desired_rate);
  pcmRate = rate;

  in.setup(rate);
  out.setup(rate);

// synchronize the two alsa_pcm sub-devices:
  int linkerr = snd_pcm_link(in.phndl, out.phndl);
  if (linkerr < 0) {
    cout << "Lockin: synchronization link failed: "
        << snd_strerror(linkerr)
        << endl;
  }

// Be sure to configure mixer  _after_ opening the pcm handlers,
// because we want to hold exclusive access to the mixer (control
// device) but snd_pcm_open wants to temporarily open the control
// device for its own reasons.

  config_mixer(foo.mixer_ctlfile, foo.justcard);

  foo.open_aux_io();           // checkfile, mostly

  prog_start_time = out.now();
  if (verbosity) {
    fprintf(stderr, "starting %s" NL, progname.c_str());
  }

  app.setStyle("plastique");
  QWidget ancestor;
  QwtPlot asdf(&ancestor);
// myWindow is derived from QWidget .... class myWindow : public QWidget
  myWindow topWindow;
  topWindow.ctrlCol->refOutGroup->freqBox->setValue(foo.reffreq);
  topWindow.actualFreqs();                // initialize actual freqs

  topWindow.show();

  int pipefd[2];
  if (pipe(pipefd)) {
    fprintf(stderr, "Could not open pipe: ");
    perror(0);
    exeunt(1);
  }

  ref_arg refarg(foo.refival, &out, pipefd[1/* writing */]);
  pthread_t refout_id;
  pthread_create(&refout_id, 0, &refout, &refarg);

  pthread_t krunch_id;
  krunch_arg karg(&topWindow, &in, &out, &foo, pipefd[0/* reading */]);
  pthread_create(&krunch_id, 0, &krunch, &karg);

  int rslt = app.exec();
  cout << "Qt returns: " << rslt << endl;

  exeunt(0);
}

string purify(const string foo) {
  string rslt;
  for (string::size_type ii = 0; ii < foo.length(); ii++) {
    char ch = foo[ii];
    if (isupper(ch)) ch += 'a' - 'A';
    if (isalnum(ch)) rslt += ch;
  }
  return rslt;
}

// return a string like "usbaudio.ctl"
// based on ALSA's notion of the DRIVER name
string invent_driverfile(const string justcard){
    string driver("unknown");
    snd_ctl_t* chndl;           // control handle
    string xxx = "hw:" + justcard;
    int err = snd_ctl_open(&chndl, xxx.c_str(), SND_CTL_RDONLY);
    if (err < 0) {
      chndl = 0;
      cerr << "Error opening control channel to card "
          << justcard << endl;
      // proceed with driver = "unknown"
    } else {
      snd_ctl_card_info_t *info;
      snd_ctl_card_info_t **info_p;  /* defeat paranoid compiler warning */
      info_p = &info;
      snd_ctl_card_info_alloca(info_p);
      err = snd_ctl_card_info(chndl, info);
      if (err < 0) {
        cerr << "Can't get card info: " << err << endl;
      } else {
#if 0
        int wid(18);
        cerr << left;
  // example: hw:0
        cerr << setw(wid) << "Ctl name:"
                << snd_ctl_name(chndl) << endl;

  // example: card0 (but could be changed in modules.conf):
        cerr << setw(wid) << "Card ID:"
                << snd_ctl_card_info_get_id(info) << endl;

  // example:  Sound Blaster Extigy
        cerr << setw(wid) << "Card name:"
                << snd_ctl_card_info_get_name(info) << endl;

  // example:  M Audio Delta 1010 at 0x1400, irq 22
        cerr << setw(wid) << "Card longname:"
                << snd_ctl_card_info_get_longname(info) << endl;

  // example: ICE1712
        cerr << setw(wid) << "Driver:"
                << snd_ctl_card_info_get_driver(info) << endl;

        cerr << setw(wid) << "Mixer name:"
                << snd_ctl_card_info_get_mixername(info) << endl;
#endif
        driver = snd_ctl_card_info_get_driver(info);
      }
    }
    return purify(driver) + ".ctl";
}

// return a string like "soundblasterextigy.ctl"
// based on ALSA's notion of the CTL name
string invent_ctlfile(const string justcard){
    string driver("unknown");
    snd_ctl_t* chndl;           // control handle
    string xxx = "hw:" + justcard;
    int err = snd_ctl_open(&chndl, xxx.c_str(), SND_CTL_RDONLY);
    if (err < 0) {
      chndl = 0;
      cerr << "Error opening control channel to card "
          << justcard << endl;
      // proceed with driver = "unknown"
    } else {
      snd_ctl_card_info_t *info;
      snd_ctl_card_info_t **info_p;  /* defeat paranoid compiler warning */
      info_p = &info;
      snd_ctl_card_info_alloca(info_p);
      err = snd_ctl_card_info(chndl, info);
      if (err < 0) {
        cerr << "Can't get card ID info: " << err << endl;
      } else {
        driver = snd_ctl_card_info_get_name(info);
      }
    }
    return purify(driver) + ".ctl";
}

static string digits = "0123456789";

void dump_io() {
  for (int ii = 0; ii<getdtablesize(); ii++){
    int rslt = fcntl(ii, F_GETFL);
    if (rslt != -1) {
      cout << "FD " << ii << " flags " << rslt << endl;
    } else if (0) {
      cout << "FD " << ii << " errno " << errno
                  << " == " << strerror(errno) << endl;
    }
  }
}

// Process command-line switches, et cetera
void snark::cmdline(int argc, char **argv) {

// Note basic default values were set up in constructor.
// Now process options:
  int ch;
  time_t mytime;
  struct tm* now;
  const int ALT(128);
  static struct option long_options[] = {
    {"buffer-size",     1, NULL, 'b'},
    {"card",            1, NULL, 'c'},
    {"help",            0, NULL, 'h'},
    {"mixer-ctl",       1, NULL, 'm'},
    {"output-fifo",     1, NULL, 'o'},
    {"verbose",         0, NULL, 'v'},
    {"write",           1, NULL, 'w'},
    {"amplitude",       1, NULL, 'A'},
    {"Amplitude",       1, NULL, 'A'},
    {"channel-mask",    1, NULL, 'C'},
    {"Channel-mask",    1, NULL, 'C'},
    {"frequency",       1, NULL, 'F'},
    {"Frequency",       1, NULL, 'F'},
    {"delta-f",         1, NULL, 'D'},
    {"delta-f",         1, NULL, 'D'},
    {"Vo-cal",          1, NULL, 'O'},
    {"Vi-cal",          1, NULL, 'I'},
    {"Time-shift",      1, NULL, 'T'},
    {"time-shift",      1, NULL, 'T'},
    {"Phase-shift",     1, NULL, 'P'},
    {"phase-shift",     1, NULL, 'P'},
    {"kout",            1, NULL, 'K'},
    {"Kout",            1, NULL, 'K'},
    {"frame-rate",      1, NULL, 'N'},
    {"quantum-sig",     1, NULL, 'Q'},
    {"Quantum-sig",     1, NULL, 'Q'},
    {"rin",             1, NULL, 'R'},
    {"Rin",             1, NULL, 'R'},
    {"zref",            1, NULL, 'Z'},
    {"Zref",            1, NULL, 'Z'},
    {"1pass",           0, NULL, '1'},
    {"xrun-verbose",    0, NULL, ALT|'x'},
    {"hack",            0, NULL, ALT|'h'},
    {"snooze",          1, NULL, ALT|'s'},
    {NULL,              0, NULL, 0}
  };

//////////////////////////////////////////////////////////////////////

  mytime = time(0);
  srand(mytime);
  mixer_ctlfile = "<>";         // fill in default, later
  now = localtime (&mytime);
  if (now) {};
  int helpme(0);

// Process commandline options
  while(1) {
//    int getopt_long(int argc, char * const argv[],
//                  const struct option *longopts, int *longindex);

    ch = getopt_long (argc, argv, long_options, NULL);
    if (ch == -1)
      break;

    if (optarg) if (*optarg == ':' || *optarg == '=') optarg++;
    switch(ch) {
      case '1':
        onepass = 1;
        break;
      case 'b':
        nframe = atoi(optarg);
        break;
      case 'c':
        card_device = optarg;
        if (card_device == "?"
          || card_device == ""
          || card_device == "-1") {
          device_list(SND_PCM_STREAM_CAPTURE);
          exit(0);
        }
        if (card_device == "-2") {
          device_list(SND_PCM_STREAM_PLAYBACK);
          exit(0);
        }
        break;
      case 'm':
        mixer_ctlfile = optarg;
        break;
      case 'w':
        wcheck_fname = optarg;
        break;
      case 'N':
        desired_rate = atoi(optarg);
        break;
      case 'C':
        channel_mask = atoi(optarg);
        break;
      case 'K':
        kout = atof(optarg);
        break;
      case 'Z':
        zref = atof(optarg);
        break;
      case 'F':
        reffreq = atof(optarg);
        break;
      case 'I':
        ViCal_dB = atof(optarg);
        break;
      case 'O':
        VoCal_dB = atof(optarg);
        break;
      case 'T':
        timeShift = atof(optarg);
        break;
      case 'P':
        phaseShift = atof(optarg);
        break;
      case 'A':
        refOutAmp_dB = atof(optarg);
        break;
      case 'D':
        refival = atof(optarg);
        break;
      case 'v':
        verbosity ++;
        break;
      case 'h':
        helpme++;
        goto helper;
      case ALT|'x':
        xrun_verbosity++;
        break;
      case ALT|'h':
        dump_io();
        exit(0);
        break;
      case ALT|'s':
        snooze = atof(optarg);
        break;
      case '?':         // optarg() uses this for any unrecognized
                        //   option, and has already complained about it.
        cerr << "For help, try\n  " << argv[0]
             << " --help" << endl;
        exit(1);
      default:
        int chx(ch&~ALT);
        fprintf(stderr, "Sorry, option %s'%c' not implemented.\n",
                ch!=chx? "ALT+" : "", chx);
        exit(1);
    }
  }
helper:;;;;

  if (!helpme) if (optind < argc) {
    printf ("Extraneous verbiage: ");
    while (optind < argc) cerr << argv[optind++] << " " ;
    cerr << endl;
    exit(1);
  }

  string::size_type where = card_device.find_first_not_of(digits);
  if (where == card_device.npos) justcard = card_device;
  else justcard = card_device.substr(0, where);

  if(helpme) {
    usage(0);
    exit(0);            // no need to exeunt()
  }

}// cmdline


void config_mixer(const string ctlfilestr, const string justcard){
  if (!ctlfilestr.length()) return;

  string ctlfile = fixup_ctlfile(ctlfilestr, justcard);

// probe for existence:
  int rslt = open(ctlfile.c_str(), O_RDONLY);
  if (rslt < 0) {
    fprintf(stderr, "Could not open alsactl file '%s': %m\n",
                ctlfile.c_str());

    string mixerfile = fixup_ctlfile(ctlfilestr, justcard, 1).c_str();
    int retry = open(mixerfile.c_str(), O_RDONLY);
    if (retry < 0) {
      fprintf(stderr, "... will use inherited mixer settings.\n");
      return;
    }
    fprintf(stderr, "... using fallback: %s\n", mixerfile.c_str());
    ctlfile = mixerfile;
    close(retry);
  } else close(rslt);          // probe has been completed


#ifdef ALSAHACK
  int load = load_state(ctlfile.c_str(), justcard.c_str(), 1, 1);

  if (load) {
    fprintf(stderr, "error in alsa load_state ... " NL);
    fprintf(stderr, " ... using ctlfile '%s' " NL, ctlfile.c_str());
    fprintf(stderr, " ... using card '%s' " NL,   justcard.c_str());
    exit(1);
  }
#endif
}

// Open miscellaneous i/o features:
void snark::open_aux_io(){
  if (wcheck_fname.length()) {
    wcheck_fd = open(wcheck_fname.c_str(), O_WRONLY|O_CREAT|O_TRUNC, 0600);
    if (wcheck_fd == -1) {
      fprintf(stderr, "Couldn't open checkfile for writing: %m" NL);
      exit(1);
    }
  }
}

void snark::printx(
 const int colored,
 const char* name,
 const double foo_1,
 const double foo_2,
 double plogp,
 const alsa_pcm* /*pcm*/
) {

  double stdev = sqrt(foo_2 - foo_1*foo_1);

  double dbdev(-999);
  if (stdev != 0.0) dbdev = 20 * log10(stdev / full_swing_2);
  if (colored) cout << crtRed;
  printf("%6s: 1st %9.3g, 2nd %9.3g, stdev %9.3g "
          "= %7.3fdB, plp %6.3f",
        name, foo_1, foo_2, stdev, dbdev, plogp);
// switch back to black BEFORE printing the newline;
// otherwise bad things happen when piping to /bin/head
  if (colored) cout << crtNormal;
  cout << endl;
}

const double microsec(1e-6);

void cleanup(int sts){
          for (string::size_type ii = 0;  ii < clean_me.size(); ii++) {
    int pid = clean_me[ii];
    if (pid > 0) kill(pid, SIGQUIT);
  }
  cout << crtNormal;
  exit(sts);
}

void trap_exit(int signum) {
  fprintf(stderr, "%s stopping on signal %i" NL, progname.c_str(), signum);
  cleanup(0);
}

void discard(int signum) {
  fprintf(stderr, "Discarding signal %i" NL, signum);
}

void exeunt(int sts){
  fprintf(stderr, "%s exiting, status %i" NL, progname.c_str(), sts);
  cleanup(sts);
}

int dummy(const int xx) {
  return xx;
}