/*
 Subroutines for data acquisition control and processing
 
 Copyright (c) 2008 Dimtel, Inc., All Rights Reserved

 $Id: subUSBio.c,v 1.4 2013/11/12 01:01:19 dim Exp $
 $Date: 2013/11/12 01:01:19 $
 $Author: dim $
 $Revision: 1.4 $
 $Log: subUSBio.c,v $
 Revision 1.4  2013/11/12 01:01:19  dim
 Proper include for updated usbio

 Revision 1.3  2012/11/11 05:48:58  dim
 Updates in 2009-2012 range

 Revision 1.2  2009/06/07 16:50:29  dim
 Added AD9512 soft synchronization

 Revision 1.1.1.1  2009/06/02 20:25:59  dim
 Initial import


*/

#include <dbDefs.h>
#include <registryFunction.h>
#include <subRecord.h>
#include <aSubRecord.h>
#include <dbAccess.h>
#include <epicsExport.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <complex.h>
#include <float.h> /* DBL_EPSILON */
#include <string.h>
#include <sys/time.h>
#include <pthread.h>

/* Local includes */
#include "LLRF.h"
#include "dacq.h"
#include "usbio.h"
#include "map.h"
#include "cmath.h"

static int usb_initialized = 0;
static usbio_context *sys;
static pthread_mutex_t ctxt_mutex = PTHREAD_MUTEX_INITIALIZER;

/* subUSBbeamread acquires data and places it in the snapshot */
acq_data snapshot;
pthread_mutex_t snapshot_mutex = PTHREAD_MUTEX_INITIALIZER;

int USB_Debug = 0;

typedef long (*processMethod)(subRecord *precord);

static long subUSBinit(subRecord *psub, processMethod process)
{
  int rc;
  
/*  printf("subUSBinit: thread id 0x%08x\n", pthread_self()); */
  pthread_mutex_lock(&ctxt_mutex);

  if (usb_initialized == 0)
  {
    sys = usbio_open(0);
    if (sys != NULL)
      usb_initialized = 1;
  }
  rc = (sys == NULL);

  pthread_mutex_unlock(&ctxt_mutex);
  
  if (USB_Debug == 2)
  {
    printf("Record %s called subUSBinit(%p)\n",
            psub->name, (void*) psub);
    printf("subUSBinit: usbio_open() returned 0x%x, usb_initialized=%d\n", 
           (int)sys, usb_initialized);
  }
  
  if (rc == 0)
  {
    unsigned x = 0;
    /* Reset test mode on AD5644 */
    usbio_block_write(sys, AD5644_A_TEST, &x, 1);
  }
    
  return rc;
}

static long subUSBwrite(subRecord *psub)
{
  /*
   * Inputs:  A = Value to write
   *          B = Address to write
   *          D = FLAG:
   *              bit 7:   readback disable (write only)
   *              bit 6:   unsigned select
   *              bit 4-0: sign extension width
   * Outputs: C = Readback
   *          VAL = 0 ok/1 nfg
   */
   
  int flag_rdbk     = (int)psub->d & 0x80;
  int flag_unsigned = (int)psub->d & 0x40;
  int flag_sex      = (int)psub->d & 0x1f;
  unsigned int val, in = (unsigned)psub->a;
  int addr = (int) psub->b, rc, mask;
 
  if (USB_Debug == 2)
  {
    printf("Record %s called subUSBwrite(%p)\n",
           psub->name, (void*) psub);
    printf("address = 0x%06x; val = 0x%08x\n", addr, in);
  }
  rc = usbio_block_write(sys, addr, &in, 1);
  if (rc != 1)
  {
    if (USB_Debug == 1)
      printf("Write error, resetting USB IO (1)\n");
    usbio_if_reset(sys);
  }
  if (flag_rdbk == 0)
  {
    rc = usbio_read(sys, addr, &val);
    if (rc != 1)
    {
      if (USB_Debug == 1)
        printf("Read error, resetting USB IO\n");
      usbio_if_reset(sys);
    }
    
    if (flag_unsigned == 0) /* Signed result, perform sign extension */
    {
      mask     = (-1)<<(flag_sex-1);
      psub->c  = (double)(((int)val + mask)^mask);
    }
    else
      psub->c = (double) val;
  
    if (USB_Debug == 2)
      printf("Readback = 0x%08x\n", val);

    if ((rc == 1) && (in == (unsigned)psub->c))
      psub->val=0;
    else  
      psub->val=1;
  }
  else
  {
    psub->val = 0;
    psub->c   = in;
  }

  return 0;
}

static long subUSBread(subRecord *psub)
{
  /*
   * Inputs:  A = Address to read
   *          C = FLAG:
   *              bit 7:   readback disable (write only)
   *              bit 6:   unsigned select
   *              bit 4-0: sign extension width
   * Outputs: VAL = B = Readback
   *          
   */
   
  int flag_unsigned = (int)psub->c & 0x40;
  int flag_sex      = (int)psub->c & 0x1f;
  unsigned int val;
  int addr = (int)psub->a, rc, mask;
  
  rc = usbio_read(sys, addr, &val);
  if (rc != 1)
   {
    if (USB_Debug == 1)
      printf("Read error, resetting USB IO\n");
    usbio_if_reset(sys);
  }

  if (flag_unsigned == 0) /* Signed result, perform sign extension */
  {
    mask      = (-1)<<(flag_sex-1);
    psub->val = psub->b = (double)(((int)val + mask)^mask);
  }
  else
    psub->val = psub->b = (double) val;
  
  if (USB_Debug == 2)
   {
        printf("Record %s called subUSBread(%p)\n",
               psub->name, (void*) psub);
        printf("address = 0x%07x; val = 0x%04x\n", addr, (int)psub->b);
   }

  return(0);
}

static long subUSBdatard(subRecord *psub)
{
  /*
   * Inputs:  C = Enable
   * Outputs: VAL = 0 - OK, 1 - file open failed, 2 - write failed,
   *          3 - USB read failed
   */
  
  char *fname;
  int fd; 
 
  if (USB_Debug == 2)
  {
        printf("Record %s called subUSBblockread(%p)\n",
               psub->name, (void*) psub);
        printf("enable = %d\n", (int)psub->c);
  }
  
  if ((int)psub->c == 1)
    return 0;
  
  fname = getenv("IGP_FILE");
  fd = open((fname == NULL) ? "rfile.dat" : fname, O_WRONLY|O_CREAT|O_TRUNC, 0644);
  
  if (fd == -1)
  {
    if (USB_Debug >= 1)
      printf("File open failed\n");
    psub->val = 1;
    return 0;
  }

  if (write(fd, snapshot.data, ADC_LENGTH*2*sizeof(int)) !=  ADC_LENGTH*2*sizeof(int))
  {
    close(fd);
    psub->val = 2;
    return 0;
  }

  close(fd);
  psub->val=0;
  return 0;
}

typedef enum acq_state {
  INITIAL,
  IDLE,
  WAITING_FOR_DATA,
  READING,
  DATA_READY
} acq_state;

static long subUSBacq(subRecord *psub)
{
  /*
   * Inputs:  A = Trigger selector (0 - int)
   *          B = DRA (data-read arming)
   *	      C = Enable
   *          D = Single acquisition enable
   *
   */
  
  static acq_state state = INITIAL;
  unsigned dtrig[10];
  unsigned ram_block;
  int k, diff, rc;
  unsigned atrig[10];
  int dra;
         
  /* Persistent local storage */
  static unsigned ds[BLOCKRAM_LENGTH];
         
  /* Timing structures */
  struct timeval t1, t2;

  if (USB_Debug == 2)
  {
        printf("Record %s called subUSBbeamread(%p)\n",
               psub->name, (void*) psub);
  }

  if (USB_Debug == 1)
    gettimeofday(&t1, NULL);
    
  /* Data acquisition and readback state machine */
  while (1)
  {
    switch (state)
    {
      case (INITIAL):
        /* First time this subroutine is called. Here we perform all
           initializations and transition to IDLE */
        snapshot.new = 0;
        state = IDLE;
        break;        
      case (IDLE):
        /* No acquisition has been started, no readback in progress. Check
           if enabled, start an acquisition */

        if ((int)psub->c != 0) /* Enabled */
        {
          dra = (int)psub->b; /* Arm after readout bit */
          
          k = 0;

          /* Trigger the acquisition */
          atrig[k]   = CSR_A_TRIG;
          dtrig[k++] = 0; /* Does not matter */
  
          /* Arm it, if necessary */
          if (dra)
          {
            atrig[k]   = CSR_A_ARM;
            dtrig[k++] = 0; /* Does not matter */
          }
          rc = usbio_mult_write(sys, atrig, dtrig, k);
          if (rc != k)
          {
            if (USB_Debug == 1)
              printf("Write error, resetting USB IO (2)\n");
            usbio_if_reset(sys);
          }
  
          usleep(T_ACQ_WAIT);
          
          state = WAITING_FOR_DATA;
        }
        else
          return 0;
        break;
      case (WAITING_FOR_DATA):
        if (0) /* Not implemented yet - FIX */
        {
          /* Check the memory status bit */
/*          rc = usbio_read(sys, CSR_A_DPBLK, &ram_block); */
          if (ram_block)
            return 0;
          else
            state = READING;
        }
        else
          state = READING;
        break;
      case (READING):
        rc = usbio_block_read(sys, BLOCKRAM, ds, BLOCKRAM_LENGTH);
        if (rc != BLOCKRAM_LENGTH)
        {
          usbio_if_reset(sys);
          if (USB_Debug == 1)
            printf("USB block read error, returned %d, asked for %d\n", rc, BLOCKRAM_LENGTH);
          state = IDLE;
          return 0;
        }
        state = DATA_READY;
        break;
      case (DATA_READY):
        if ((int)psub->d != 0)
        /* Single acquisition - reset enable */
        {
          char enable = 0;
          dbPutLink(&psub->inpc, DBR_CHAR, &enable, 1);
          usleep(1000);
        }
        /* Got everything */
        pthread_mutex_lock(&snapshot_mutex);
        snapshot.data = ds;
        for (k = 0; k < ADC_LENGTH; k++)
        {
          snapshot.adc[0][k] = (ds[k]                    & 0xffff);
          snapshot.adc[1][k] = ((ds[k] >> 16)            & 0xffff);
          snapshot.adc[2][k] = (ds[k+ADC_LENGTH]         & 0xffff);
          snapshot.adc[3][k] = ((ds[k+ADC_LENGTH] >> 16) & 0xffff);
        }
        snapshot.new = 1;
        pthread_mutex_unlock(&snapshot_mutex);
        state = IDLE;
        if (USB_Debug == 1)
        {
          gettimeofday(&t2, NULL);
          diff = deltaT(&t1, &t2);
          printf("Elapsed time (beamread): %.3f ms\n", ((double)diff)/1000.0);
        }
        return 0;
    }
  }  
}

static long subUSBad9512(aSubRecord *pgsub)
{
  uchar bypass, iramp, cramp, delay;
  uchar out0pd, out0level;
  uchar out3pd, out3curr, out3logic, out3inv;
  uchar out4pd, out4curr, out4logic, out4inv;
  uchar divide, adc_div, dac_div, dac_div_bypass;
  uchar div3, out3_div, out3_div_bypass;
  unsigned addr[AD9512_LENGTH+2], data[AD9512_LENGTH+2];
  int idx = 0, k;
  
  bypass    = *(unsigned char *)pgsub->a;
  iramp     = *(unsigned char *)pgsub->b;
  cramp     = *(unsigned char *)pgsub->c;
  delay     = *(unsigned char *)pgsub->d;
  out0pd    = *(unsigned char *)pgsub->e;
  out0level = *(unsigned char *)pgsub->f;
  out3pd    = *(unsigned char *)pgsub->g;
  out3curr  = *(unsigned char *)pgsub->h;
  out3logic = *(unsigned char *)pgsub->i;
  out3inv   = *(unsigned char *)pgsub->j;
  out4pd    = *(unsigned char *)pgsub->k;
  out4curr  = *(unsigned char *)pgsub->l;
  out4logic = *(unsigned char *)pgsub->m;
  out4inv   = *(unsigned char *)pgsub->n;
  divide    = *(unsigned char *)pgsub->o;
  div3      = *(unsigned char *)pgsub->p;
  
  addr[idx] = AD9512_A_BYPASS; data[idx++] = bypass;
  addr[idx] = AD9512_A_RAMP; data[idx++]   = iramp << AD9512_RAMP_B_IRAMP | cramp << AD9512_RAMP_B_CRAMP;
  addr[idx] = AD9512_A_DELAY; data[idx++]	= delay << AD9512_DELAY_B_DELAY;
  addr[idx] = AD9512_A_OUT0; data[idx++]   = out0level << AD9512_OUT012_B_LEVEL | out0pd << AD9512_OUT012_B_PD;
  addr[idx] = AD9512_A_OUT1; data[idx++]   = AD9512_V_OUT012;
  addr[idx] = AD9512_A_OUT2; data[idx++]   = AD9512_V_OUT012;
  addr[idx] = AD9512_A_OUT3; data[idx++]   = out3pd    << AD9512_OUT34_B_PD |
                          out3curr  << AD9512_OUT34_B_CURR |
                          out3logic << AD9512_OUT34_B_LOGIC |
                          out3inv   << AD9512_OUT34_B_INV;
  addr[idx] = AD9512_A_OUT4; data[idx++]   = out4pd    << AD9512_OUT34_B_PD |
                          out4curr  << AD9512_OUT34_B_CURR |
                          out4logic << AD9512_OUT34_B_LOGIC |
                          out4inv   << AD9512_OUT34_B_INV;
  addr[idx] = AD9512_A_CLKSEL; data[idx++] = AD9512_V_CLKSEL;
  /* Configure divide ratios */
  adc_div = (divide-1)<<4 | (divide-1);
  if (divide == 1)
  {
    /* Use bypass */
    dac_div        = AD9512_V_DIV;
    dac_div_bypass = 1;
  }
  else
  {
    dac_div        = (divide/2-1)<<4 | (divide-1)>>1;
    dac_div_bypass = 0;
  }

  if (div3 == 1)
  {
    out3_div        = AD9512_V_DIV;
    out3_div_bypass = 1;
  }
  else
  {
    out3_div        = (div3/2-1)<<4 | (div3-1)>>1;
    out3_div_bypass = 0;
  }

  addr[idx] = AD9512_A_DIV0; data[idx++]   = adc_div;
  addr[idx] = AD9512_A_DIV1; data[idx++]   = AD9512_V_DIV;
  addr[idx] = AD9512_A_DIV2; data[idx++]   = AD9512_V_DIV;
  addr[idx] = AD9512_A_DIV3; data[idx++]   = out3_div;
  addr[idx] = AD9512_A_CFG3; data[idx++]   = out3_div_bypass << AD9512_CFG_B_BYPASS;
  addr[idx] = AD9512_A_DIV4; data[idx++]   = dac_div;
  addr[idx] = AD9512_A_CFG4; data[idx++]   = dac_div_bypass << AD9512_CFG_B_BYPASS;
  addr[idx] = AD9512_A_SYNC; data[idx++]    = 1 << AD9512_SYNC_B_SOFT;
  addr[idx] = AD9512_A_UPD;  data[idx++]    = 1;
  addr[idx] = AD9512_A_SYNC; data[idx++]    = 0;
  addr[idx] = AD9512_A_UPD;  data[idx++]    = 1;
  
  /* Add base address */
  for (k=0; k < idx; k++)
    addr[k] += CSR_A_AD9512;
    
  usbio_mult_write(sys, addr, data, idx);
  
  return 0;
}

static long subUSBpoll(aSubRecord *pgsub)
{
  /*
   * Inputs:  A = Scalar decimation factor (int)
   *          B = Scalar shift gain (int)
   *          C = sin(theta) scaling factor
   *
   * Outputs: A = Raw RMS (int x4)
   *          B = Processed RMS (double x4)
   *          C = Raw I (int x4)
   *          D = Raw Q (int x4)
   *          E = Processed I (double x4)
   *          F = Processed Q (double x4)
   *          G = Amplitude in counts (double x4)
   *          H = Phase in degrees (double x4)
   *          K = MCP3208 vector
   *	      L = DS1822 ROM vector (LSB, MSB)
   *          M = DS1822 temperature
   *          N = DS1822 presence bit
   *
   */
  unsigned decim, shift, ch;
  int data[SCALAR_LENGTH];
  double *p_rms, *p_I, *p_Q, *p_amp, *p_ph, iq_scale;
  double complex z;
  
  decim    = *(unsigned *)pgsub->a;
  shift    = *(unsigned *)pgsub->b;
  iq_scale = 1.0 / *(double *)pgsub->c;
  
  /* Read the memory */
  usbio_block_read(sys, SCALAR_A_BASE, (unsigned *)data, SCALAR_LENGTH);
  
  /* Copy out raw RMS */
  memcpy(pgsub->vala, data, 4*sizeof(int));
  
  /* Copy out raw I/Q */
  for (ch = 0; ch < 4; ch++)
  {
    *((int *)pgsub->valc + ch) = data[8 + ch * 2];
    *((int *)pgsub->vald + ch) = data[9 + ch * 2];
  }
  
  /* Copy out MCP3208 data */
  memcpy(pgsub->valk, data + MCP3208_A_CH0 - SCALAR_A_BASE, 8*sizeof(int));
  
  /* Get DS1822 data out */
  memcpy(pgsub->vall, data + (DS1822_A_ROM0 - SCALAR_A_BASE), 2*sizeof(int));
  *(unsigned *)pgsub->valm = data[DS1822_A_TEMP - SCALAR_A_BASE];
  *(int *)pgsub->valn = data[DS1822_A_STAT - SCALAR_A_BASE];
  
  /* Copy out the frequency counter value */
  *(int *)pgsub->valo = data[FREQCNT_A_VAL - SCALAR_A_BASE];
  
  /* Copy out fan monitor vector */
  memcpy(pgsub->valp, data + (FANMON_A_BASE - SCALAR_A_BASE),
         sizeof(int)*FANMON_LENGTH);
  
  /* Compute the RMS in ADC counts, convert I/Q */
  p_rms = pgsub->valb; p_I = pgsub->vale; p_Q = pgsub->valf;
  p_amp = pgsub->valg; p_ph = pgsub->valh;
  for (ch = 0; ch < 4; ch++)
  {
    *p_rms++ = sqrt((double)data[ch]*(((double)(1<<(23 - shift)))/decim));
    *p_I     = (double)data[8 + ch*2]*(iq_scale/decim*2048);
    *p_Q     = (double)data[9 + ch*2]*(iq_scale/decim*2048);
    /* p_Q is evaluated 4 times in the assignment!!! */
    z        = *p_I + I**p_Q; p_I++; p_Q++;
    *p_amp++ = cabs(z);
    *p_ph++  = carg_loc(z)/M_PI*180.0;
  }  
  return 0;
}

#define PRINT_HZ	0
#define PRINT_mHZ	1

static void printFrequency(char *output, double freq, char units)
{
  long long fr;
  long frac, mHz, Hz, kHz, MHz;

  if (units == PRINT_HZ)
  {
    /* Round to 0.1 Hz */
    fr= (long long)(freq*10.0 + 0.5);
    frac = fr % 10;
    Hz  = (fr/10) % 1000;
    kHz = (fr/10000) % 1000;
    MHz = fr/10000000;
  
    /* Create the string */
    if (MHz == 0)
    {
      if (kHz == 0)
        sprintf(output, "%3ld.%01ld Hz", Hz, frac);
      else
        sprintf(output, "%3ld,%03ld.%01ld Hz", kHz, Hz, frac);
    }
    else
        sprintf(output, "%3ld,%03ld,%03ld.%01ld Hz", MHz,
            kHz, Hz, frac);
  }
  else
  {
    fr = (long long)(freq*1000.0 + 0.5);
    mHz = fr % 1000;
    Hz  = (fr/1000) % 1000;
    kHz = (fr/1000000) % 1000;
    MHz = fr/1000000000;
    
    /* Create the string */
    if (MHz == 0)
    {
      if (kHz == 0)
      {
        if (Hz == 0)
          sprintf(output, "%3ld mHz", mHz);
        else
          sprintf(output, "%3ld,%03ld mHz", Hz, mHz);
      }
      else
        sprintf(output, "%3ld,%03ld,%03ld mHz", kHz, Hz, mHz);
    }
    else
        sprintf(output, "%3ld,%03ld,%03ld,%01ld mHz", MHz,
            kHz, Hz, mHz);
  }
}


static long subUSBDriveGen(aSubRecord *pgsub)
{
  /*
   * Inputs:  A = Sampling frequency (MHz)
   *          B = Waveform (0-sin, 1-sqr, 2-saw, 3-arb)
   *          C = Set frequency (kHz)
   *          D = Amplitude
   *          E = Arbitrary waveform
   *
   * Outputs: A = Actual frequency (kHz)
   *          B = Actual frequency string
   *          C = Frequency error string
   *
   */

  int data[DRIVERAM_LENGTH*2];
  unsigned int  mem_image[DRIVERAM_LENGTH];
  int wfm, k, m, period, rv, M0, M_L, M_max, M_H, M;
  int length = DRIVERAM_LENGTH*2;
  double freq, ampl, fs, fr1[2];

  /* Read input data */
  fs   = *(double *)pgsub->a * 2.0e6; /* DAC is DDR */
  wfm  = *(long *)  pgsub->b;
  freq = *(double *)pgsub->c * 1.0e3;
  ampl = *(double *)pgsub->d;

  if (fs == 0)
    return 0;

  if (freq > fs/2)
    freq = fs/2;

  M0 = floor(freq*(length/fs) + 0.5);
/*  printf("freq=%g; length=%d; Fs=%g; period %d\n", freq, length, Fs, period);
*/
  if (M0 <= 0) M0 = 1;
  
  if (wfm == WFM_SIN)
    M = M0;
  else
  {
    for (m=M0; m >= 1; m--)
      if (length % m == 0)
      {
        if (wfm == WFM_SQUARE && length/m % 2 == 0)
          break;
        else if (wfm != WFM_SQUARE)
          break;
      }
      M_L = m;
    M_max = 2*M0 - M_L;
    M_H = 0;
  
    for (m = M0; m < M_max; m++)
      if (length % m == 0)
      {
        if (wfm == WFM_SQUARE && length/m % 2 == 0)
        {
          M_H = m;
          break;
        }
        else if (wfm != WFM_SQUARE)
        {
          M_H = m;
          break;
        }
      }
    if (M_H == 0)
      M = M_L;
    else
    {
      fr1[0] = fabs(M_L*fs/length - freq);
      fr1[1] = fabs(M_H*fs/length - freq);
      if (fr1[0] < fr1[1])
        M = M_L;
      else
        M = M_H;
    }
/*    printf("M_L = %d, M_H = %d\n", M_L, M_H); */
  }
  
  /* Report the frequency */
  *(double *)pgsub->vala = M * fs / length / 1.0e3;
  
  /* Print frequency and frequency error strings */
  printFrequency((char *)pgsub->valb, *(double *)pgsub->vala*1000.0, PRINT_HZ);
  printFrequency((char *)pgsub->valc, 
                 fabs(freq - *(double *)pgsub->vala*1000.0), PRINT_HZ);
  
  period = length / M;
              
/*  printf("Setting DAC pattern to %s at %.3f kHz, amplitude %.2f (%d samples per period)\n",
  (wfm == WFM_SIN) ? "sine wave" :
  (wfm == WFM_SQUARE) ? "square wave" : "sawtooth", M*Fs/length/1e3, ampl, period);
*/  
  /* Generate the pattern */
  for (k=0; k < DRIVERAM_LENGTH*2; k++)
    switch (wfm)
    {
      case WFM_SIN:
        data[k] = (int)(DAC_FS*ampl*cos(2*M_PI*k*M/length));
        break;
      case WFM_SQUARE:
        if (k % period < period/2)
          data[k] = DAC_FS*ampl;
        else
          data[k] = -DAC_FS*ampl;
        break;
      case WFM_SAWTOOTH:
        data[k] = (int)(DAC_FS*(-ampl + 2*ampl*(k % period)/(period-1)) + 0.5);
        break;
      case WFM_ARB:
        data[k] = *((short *)pgsub->e + k);
    }

  
  {
    int fd;
  
    fd = open("arb.dat", O_WRONLY|O_CREAT|O_TRUNC, 0644);
    write(fd, data, 2*DRIVERAM_LENGTH*sizeof(int));
    close(fd);
  }

    
  for (k = 0; k < DRIVERAM_LENGTH*2; k += 2)
    mem_image[k>>1] = ((data[k] & 0x3fff) << 16) | (data[k+1] & 0x3fff);

  rv = usbio_block_write(sys, DRIVERAM, mem_image, DRIVERAM_LENGTH);
  if (rv != DRIVERAM_LENGTH)
    usbio_if_reset(sys);
    
  return 0;
}

/* Compute rational approximation of the ratio value.
   Approximation stops when at the next step denominator N exceeds Nmax
*/
static void cfrac(double ratio, int Nmax, unsigned int *M, unsigned int *N)
{
  /* Past, current, and next values */
  int i;
  unsigned long long int M2, M1, Mn, N2, N1, Nn, a1, a2;
  double x1, x2, y1, y2;
  
  M2 = 0; N2 = 1;
  M1 = 1; N1 = 0;
  
  a1 = (unsigned int) ratio;
  x1 = 1.0;
  y1 = ratio - a1; /* fractional part */
  
  for (i = 0; i < 20; i++)
  {
    /* Next state computation */
    a2 = (unsigned int) (x1/y1);
    x2 = y1;
    y2 = x1 - a2*y1;
    Mn = a1*M1 + M2;
    Nn = a1*N1 + N2;
    if (Nn > Nmax)
      break;
    
    /* State update */
    a1 = a2; x1 = x2; y1 = y2;
    M2 = M1; M1 = Mn; N2 = N1; N1 = Nn;
    if (a1 == 0)
      break;
  }

  if (N1 == 0) /* Input was an integer */
  {
    M1 = a1;
    N1 = 1;
  }
  *M = M1;
  *N = N1;
}

static long subUSBDDSConfig(aSubRecord *pgsub)
{
  /*
   * Inputs:  A = Sampling frequency (MHz)
   *          B = Waveform (0-sin, 1-sqr)
   *          C = Set frequency (kHz)
   *          D = Amplitude
   *          E = DDS base address
   *
   * Outputs: A = Actual frequency (kHz)
   *          B = Actual frequency string
   *          C = Frequency error string
   *          D = M factor
   *          E = N factor
   *
   */

  double ratio, fsamp, freq, fr_act, amp;
  unsigned int M, N, addr[3], regs[3], amp_int, square, base;
  int coeff_I, coeff_Q;
  
  fsamp = *(double *)pgsub->a*2000.0; /* DAC frequency is 2x ADC */
  square = *(long *)pgsub->b;
  freq  = *(double *)pgsub->c;
  amp   = *(double *)pgsub->d;
  base  = *(long *)pgsub->e;
  if (freq > fsamp/2) freq = fsamp/2;  
  ratio = freq*(1<<18)/fsamp;
  
  cfrac(ratio, DDS0_V_MN, &M, &N);
  
  fr_act = ((double)M/(double)N)/(1<<18)*fsamp;

  if (square)
    amp_int = (unsigned int)(amp*DDS1_V_AMP/2);
  else
    amp_int = (unsigned int)(amp*DDS_V_AMPFS + 0.5);

  coeff_I = (int) (32767*cos((M_PI*M)/(1<<17)/N) + 0.5);
  coeff_Q = (int) (32767*sin((M_PI*M)/(1<<17)/N) + 0.5);

  addr[0] = base;
  regs[0] = (((M/N) & DDS0_V_ADV) << DDS0_B_ADV) |
            (square << DDS0_B_SQUARE) |
            (((4096-N) & DDS0_V_MN) << DDS0_B_MN);
  
  addr[1] = base+1;
  regs[1] = (((M%N) & DDS1_V_REM) << DDS1_B_REM) |
            ((amp_int & DDS1_V_AMP) << DDS1_B_AMP);
            
  addr[2] = base+2;
  regs[2] = ((coeff_I & DDS2_V_ROTI) << DDS2_B_ROTI) |
            ((coeff_Q & DDS2_V_ROTQ) << DDS2_B_ROTQ);
            
  usbio_mult_write(sys, addr, regs, (base == CSR_A_DDS0) ? 3 : 2);

  if (USB_Debug == 1)
  {
    printf("0x%02x <= 0x%08x, 0x%02x <= 0x%08x, 0x%02x <= 0x%08x\n",
           addr[0], regs[0], addr[1], regs[1], addr[2], regs[2]);
    printf("M=%d,N=%d,freq=%fkHz,fr_act=%fkHz,amp=%f/%d\n",
           M, N, freq, fr_act, amp, amp_int);
  }

  *(double *)pgsub->vala = fr_act;
  printFrequency((char *)pgsub->valb, fr_act*1.0e3, PRINT_HZ);
  printFrequency((char *)pgsub->valc, fabs(freq - fr_act)*1.0e3, PRINT_mHZ);
  *(long *)pgsub->vald = M;
  *(long *)pgsub->vale = N;

  return 0;
}

static registryFunctionRef subUSBRef[] = {
  {"subUSBinit",   (REGISTRYFUNCTION)subUSBinit},
  {"subUSBwrite",  (REGISTRYFUNCTION)subUSBwrite},
  {"subUSBread",   (REGISTRYFUNCTION)subUSBread},
  {"subUSBdatard",   (REGISTRYFUNCTION)subUSBdatard},
  {"subUSBacq",   (REGISTRYFUNCTION)subUSBacq},
  {"subUSBpoll",   (REGISTRYFUNCTION)subUSBpoll},
  {"subUSBad9512", (REGISTRYFUNCTION)subUSBad9512},
  {"subUSBDriveGen", (REGISTRYFUNCTION)subUSBDriveGen},
  {"subUSBDDSConfig", (REGISTRYFUNCTION)subUSBDDSConfig}
};

static void subUSBRegistrar(void)
{
  registryFunctionRefAdd(subUSBRef, NELEMENTS(subUSBRef));
}
epicsExportRegistrar(subUSBRegistrar);
