package nxm.dsp.prim;

import nxm.dsp.lib.Complex;
import nxm.sys.lib.Data;
import nxm.sys.lib.DataFile;
import nxm.sys.lib.Primitive;

/** Select samples from an input file that are nearest to baud center as indicated by sync
    data derived from the input file. This was ported from X-Midas.<br>

    Note: This class was moved from the UCL option tree in 3.1.0.

    @author Hoyt J. Heaton
    @author Jason M. Broussard
*/
public class amfsync extends Primitive {
  //Class variables (Used in more than one function)
  private DataFile inputFile;
  private DataFile outputFile;
  private DataFile syncOutFile;

  private Data inputBuffer;
  private Data outputBuffer;
  private Data syncOutBuffer;

  private boolean cxin = false;
  private int nget;
  private double p_start; // sync output file start time (sec)
  private double bufstart; // start time of input buffer being processed
  private double pp_delta; // sync output file time increment(sec)
  private double pstep_nominal; // nominal number of input samples for every sync output sample
  private int nextout;
  private double ifirst;
  private double indlast; // index of last point to process from in buf
  private double rfirst; //
  private int ngot;
  private int nhave;
  private int npts;
  private int ncleft; // number of input samples left in input buffer to process
  private int lfilt; // filter length used for filtering symbol line
  private int decfac; // decimation factor after filtering symbol line
  private float[][] cbufin;
  private float[][] cbufout;
  private float[] magbuf;
  private float[] bufin;
  private float[] bufout;
  private int kfilt;
  private double istart;
  private double indbase; // input buffer base index for selecting samples
  private int blksize; // size of buffer used for processing input
  private int outblk; // Output data block size(used as a hint)
  private String sync_mode;
  private int pbufsize;
  private float[] phasebuf;
  private boolean syncout;
  private float[] cfilt;
  private float[] sfilt;
  private double timelast;
  private double time_now;
  private double time_step;
  private double pri;  // symbol period (sec)
  private double xx_delta1; // input file sample increment (sec)
  private int sup_fact;
  private double x_start1; // input file start time (sec)
  private float sumr;
  private float sumi;
  private boolean warn[];

  //-- Constants
  private static final int L_BBUF = 524288;
  private static final int L_RBUF = L_BBUF / 4;
  private static final int L_CBUF = L_BBUF / 8;
  private static final int PBUFSIZE_MAX = 4096;
  private static final int REAL = 0;
  private static final int IMAG = 1;

  /** default constructor */
  public amfsync () {}

  public int open () {
    //Get command line parameters
    double brate = MA.getD("BRATE"); //symbol rate to track (Hz)
    pri = 1.0 / brate;
    lfilt = MA.getL("LFILT");
    decfac = lfilt / 4;
    sync_mode = MA.getS("SYNC_MODE");
    sup_fact = Math.max( MA.getL("SUP_FACT"), 1);

    //Set up file headers and open files.
    inputFile = MA.getDataFile ("IN", "1000", "S#,-SP,C#", 0);
    inputFile.open();
    cxin = (inputFile.getSPA() == 2);
    x_start1 = inputFile.getXStart();
    xx_delta1 = inputFile.getXDelta();
    double size1 = inputFile.getSize(); // input file size (elements)
    double size2 = size1 * xx_delta1 / pri * sup_fact; // symbol output file size (elements)

    if (lfilt < 128) {
      M.warning("AMFSYNC - BPF length increased to 128");
      lfilt = 128;
    }
    else if (lfilt > L_RBUF) {
      M.warning("AMFSYNC - BPF length too large, decreased.");
      lfilt = L_RBUF;
    }

    //Set Transfer length
    // We normally save 0.75 * lfilt, so try to add this to buffer so that
    //  the actual transfer size will match the /TL value.

    blksize = Math.min( L_BBUF / (int)(Math.max(inputFile.getBPE(), 4)) + lfilt * 3/4,
        L_BBUF / (int)(Math.max(inputFile.getBPE(), 4)) );

    if (blksize < lfilt) {
      M.error("Buffer too short for filter");
    }
    // reduce block size to filter + steps
    kfilt = (blksize-lfilt) / decfac;
    blksize = lfilt + kfilt * decfac;
    outblk = MA.getL("/OTL", -1);
    outblk = Math.min(L_RBUF, outblk);
    if (outblk < 0) {
      outblk = blksize / 2;
    }

    outputFile = MA.getDataFile("OUT", inputFile, 0);
    outputFile.setXDelta( pri / sup_fact );
    outputFile.setSize(size2);
    // xstart for output file is adjusted to a best guess of 0.5*pri after start of input file.
    outputFile.setXStart( inputFile.getXStart() + 0.5 * pri);
    outputFile.open();

    //Create sync output file if input name is not blank
    String name3 = MA.getS("SYNCOUT");
    syncout = !(name3.equals(""));

    p_start = x_start1 + (lfilt-1.0) * 0.5 * xx_delta1;
    pp_delta = xx_delta1 * (lfilt/4);
    if (syncout) {
      syncOutFile = MA.getDataFile("SYNCOUT","1000", "SF", 0);
      syncOutFile.setXStart(p_start);
      syncOutFile.setXDelta(pp_delta);
      syncOutFile.open();
    }

    // Limit pbufsize for piped output
    if (syncout && name3.charAt(0) == '_' && pp_delta != 0) {
      pbufsize = (int)(blksize * xx_delta1 / pp_delta);
      pbufsize = Math.min( Math.max(pbufsize, 2), PBUFSIZE_MAX);
    }
    else {
      pbufsize = PBUFSIZE_MAX;
    }

    //create filter for finding sync. Accumulate average so that we can subtract it to reject DC.
    double t;
    sumr = 0;
    sumi = 0;
    cfilt = new float[lfilt+1];
    sfilt = new float[lfilt+1];
    for (int i=1; i<=lfilt; i++) {
      t = (i - (lfilt + 1.0)/2.0);
      cfilt[i] = (float)((1.0 + Math.cos(t*TWOPI/lfilt)) * Math.cos(t * xx_delta1 * brate * TWOPI));
      sumr += cfilt[i];
      sfilt[i] = (float)((1.0 + Math.cos(t*TWOPI/lfilt)) * Math.sin(t * xx_delta1 * brate * TWOPI));
      sumi += sfilt[i];
    }

    //Adjust filter so it does not pass DC
    sumr = sumr / lfilt;
    sumi = sumi / lfilt;
    for (int i=1; i<= lfilt; i++) {
      cfilt[i] = cfilt[i] - sumr;
      sfilt[i] = sfilt[i] - sumi;
    }

    //initialize shared variables
    p_start = x_start1 + (lfilt - 1.0) * 0.5 * xx_delta1;
    bufstart = p_start;
    pp_delta = xx_delta1 * decfac;
    pstep_nominal = pp_delta * brate;

    //setup for initial data read
    nextout = 1;
    ifirst = 1.0;
    indlast = 0;
    rfirst = 1.0;
    ngot = 0;
    nget = 0;
    nhave = 0;
    npts = 0;
    ncleft = 0;
    timelast = x_start1 - pri;

    warn = new boolean[5];
    for (int i=1; i<=4; i++) {
      warn[i] = true;
    }

    inputBuffer = inputFile.getDataBuffer(blksize, FLOAT);
    outputBuffer = outputFile.getDataBuffer(blksize, FLOAT);
    if (syncout) {
      syncOutBuffer = syncOutFile.getDataBuffer(pbufsize, FLOAT);
    }
    cbufin   = new float[L_CBUF+1][2]; // array of complex numbers
    cbufout  = new float[L_CBUF+1][2];
    magbuf   = new float[L_RBUF+1];
    bufin    = new float[L_RBUF+1];
    bufout   = new float[L_RBUF+1];
    phasebuf = new float[PBUFSIZE_MAX + 3];

    return NORMAL;
  }

  public int process () {

    if ( (Math.round(rfirst)) >= indlast) {
      if (ngot < nget) {
        return FINISH;
      }
      //need more input data
      // Move old data to the start of the buffer
      int nsave = Math.min(lfilt-decfac, nhave);
      if (cxin) {
        if (nsave > L_CBUF) {
          M.error("nsave too large complex");
        }
        for (int i=1; i<=nsave; i++) {
          cbufin[i][REAL] = cbufin[(kfilt+1) * decfac + i][REAL];
          cbufin[i][IMAG] = cbufin[(kfilt+1) * decfac + i][IMAG];
          magbuf[i] = magbuf[(kfilt+1) * decfac + i];
        }
      }
      else {
        if (nsave > L_RBUF) {
          M.error("nsave too large real");
        }
        for (int i=1; i<=nsave; i++) {
          bufin[i]  = bufin[(kfilt+1) * decfac + i];
          magbuf[i] = magbuf[(kfilt+1) * decfac + i];
        }
      }

      ifirst  = ifirst + nhave - nsave;
      istart  = ifirst + nsave;
      indbase = ifirst - 1;
      nget    = blksize - nsave;

      //Read data
      if (cxin) {
        if (nsave + nget > L_CBUF) {
          M.error("grab too large complex");
        }
      }
      else {
        if (nsave + nget > L_RBUF) {
          M.error("grab too large real");
        }
      }
      ngot = inputFile.read(inputBuffer, nget);
      if (ngot == 0) {
        return NOOP;
      }

      if (cxin) {
        //Convert series of floats from input file into array of complex numbers.
        float[] inputWork = inputBuffer.castF(true);
        int j = nsave + 1;
        try {
          for (int i=0; i < (ngot*2); i+=2) {
            cbufin[j][REAL] = inputWork[i];
            cbufin[j][IMAG] = inputWork[i+1];
            j++;
          }
        }
        catch (ArrayIndexOutOfBoundsException e) {
          M.error("amfsync: array index of cbufin or inputWork out of bounds");
        }
        inputBuffer.uncast(inputWork, false);

        if (sync_mode.equalsIgnoreCase("REA")) {
          for (int i=nsave+1; i<=nsave+ngot; i++) {
            magbuf[i] = Math.abs(cbufin[i][REAL]);
          }
        }
        else if (sync_mode.equalsIgnoreCase("IMA")) {
          for (int i=nsave+1; i<=nsave+ngot; i++) {
            magbuf[i] = Math.abs(cbufin[i][IMAG]);
          }
        }
        else {
          for (int i=nsave+1; i<=nsave+ngot; i++) {
            magbuf[i] = Complex.cabs(cbufin[i][REAL], cbufin[i][IMAG]);
          }
        }
      }
      else { // scalar input
        float[] inputWork = inputBuffer.castF(true);
        int j=nsave+1;
        for (int i=0; i<ngot; i++) {
          bufin[j++] = inputWork[i];
        }
        inputBuffer.uncast(inputWork, false);
        for (int i=nsave+1; i<=nsave+ngot; i++) {
          magbuf[i] = Math.abs(bufin[i]);
        }
      }

      nhave = nsave + ngot;

      // indlast is the last input point that can be processed this pass
      if (ngot == nget) {
        indlast = indbase + (kfilt+2)*decfac;

      }
      else {
        //process entire buffer on last pass
        indlast = indbase + nhave;
      }
      // Now filter the input data to get sync phase
      // Is there room in phase buffer?
      if ( (npts+kfilt) >= Math.min(outblk, pbufsize)) {
        //Get array for output
        if (syncout) {
          float[] syncWork = syncOutBuffer.castF(false);
          for (int i=0; i<npts-3; i++ ) {
            if (i >= phasebuf.length-1) {
              break;
            }
            // translate 1-based phasebuf to 0-based syncWork
            syncWork[i] = phasebuf[i+1];
          }
          syncOutBuffer.uncast(syncWork, true);
          syncOutFile.write(syncOutBuffer, npts-2);
        }

        phasebuf[1] = phasebuf[npts-1];
        phasebuf[2] = phasebuf[npts];
        bufstart = bufstart + (npts-2) * pp_delta;
        npts = 2;
      }

      int klast = (nhave - lfilt) / decfac;
      for (int k=0; k<=klast; k++) {
        sumr = 0.0f;
        sumi = 0.0f;
        for (int i=1; i<=lfilt; i++) {
          sumr += ( magbuf[i + decfac * k]) * cfilt[i];
          sumi += ( magbuf[i + decfac * k]) * sfilt[i];
        }
        npts++;
        if (npts > pbufsize ) {
          M.error("store past end of phase buf");
        }
        if ((sumi == 0.0) && (sumr == 0.0)) {
          phasebuf[npts] = 0.0f;
        }
        else {
          phasebuf[npts] = (float)(-(Math.atan2(sumi, sumr)) / TWOPI);
        }

      }
    }//end if rfirst>indlast

    //We can now continue selecting points to output
    double rstep  = 0; // number of input samples to step over when extracting symbol phase.
    double rfstep = 0; // similar to rstep but includes sup_fact
    if (ncleft < 1) {
      // new call on next_clock for next constant-pri interval
      next_clock(timelast, time_now, time_step, ncleft);
      if (ncleft < 1) {
        M.warning("AMFSYNC: Serious bug after next_clock");
        ncleft = 1;
      }
      rstep = time_step / xx_delta1;
      rfstep = rstep / sup_fact;
      rfirst = (time_now - x_start1) / xx_delta1 + 1;
      timelast = time_now - time_step;
    }

    int ndleft = (int)( (indlast - Math.round(rfirst)) / rstep + 1 );
    // Number of input samples to process on a pass.
    int nloop = Math.min( ndleft, Math.min(ncleft, blksize+1-nextout));

    // Try to continue if index errors occur, but output warnings.
    if (nextout < 1) {
      if (warn[1]) {
        M.warning("Increasing nextout to 1");
        nextout = 1;
        warn[1] = false;
      }
    }
    else if ( (nextout + nloop - 1) > L_RBUF ) {
      if (warn[2]) {
        M.warning("nloop too large, reducing");
        nloop = L_RBUF - nextout + 1;
        warn[2] = false;
      }
    }

    double index;
    int j;
    if (cxin) {
      for (int i=0; i<= (nloop * sup_fact - 1); i++) {
        index = 0.5 + rfirst + i * rfstep;
        j = (int)(index - indbase);

        if (j < 1) {
          if (warn[3]) {
            M.warning("Trying to get data before start of input");
          }
          j = 1;
          warn[3] = false;
        }
        else if (j > L_RBUF) {
          if (warn[4]) {
            M.warning("Trying to get data past end of input");
          }
          j = L_RBUF;
          warn[4] = false;
        }

        if ( (nextout + i) > L_CBUF) {
          M.error("nextout too large cx");
        }

        try {
          cbufout[nextout+i][REAL] = cbufin[j][REAL];
          cbufout[nextout+i][IMAG] = cbufin[j][IMAG];
        }
        catch (ArrayIndexOutOfBoundsException e) {
          M.error("amfsync: cbufout or cbufin array out of bounds");
          return FINISH;
        }
      } // end for loop
    } // end if (cxin)
    else {
      for (int i=0; i<= (nloop * sup_fact - 1); i++) {
        index = 0.5 + rfirst + i * rfstep;
        j = (int)(index - indbase);

        if (j < 1) {
          if (warn[3]) {
            M.warning("Trying to get data before start of input");
          }
          j = 1;
          warn[3] = false;
        }
        else if (j > L_RBUF) {
          if (warn[4]) {
            M.warning("Trying to get data past end of input");
          }
          j = L_RBUF;
          warn[4] = false;
        }

        if ( (nextout + i) > L_RBUF) {
          M.error("Nextout too large re");
        }

        bufout[nextout + i] = bufin[j];
      } // end for loop
    }

    nextout = nextout + nloop;
    ncleft = ncleft - nloop;
    rfirst = rfirst + nloop * rstep;
    timelast = timelast + nloop * time_step;

    if (nextout > outblk) {
      if (nextout > blksize) M.error("nextout too big");
      if (cxin) {
        float outbuf[] = outputBuffer.castF(false);
        // Convert complex array (2d array of floats) into 1D array.
        int count = 0;
        for (int i=1; i<=(nextout-1); i++) {
          outbuf[count++] = cbufout[i][REAL];
          outbuf[count++] = cbufout[i][IMAG];
        }
        outputBuffer.uncast(outbuf, true);
      }
      else {
        float outbuf[] = outputBuffer.castF(false);
        int count = 0;
        for (int i=1; i<=nextout-1; i++) {
          outbuf[count++] = bufout[i];
        }
        outputBuffer.uncast(outbuf, true);
      }

      outputFile.write(outputBuffer, nextout-1);
      nextout = 1;
    }
    return NORMAL;
  }

  public int close () {
    // Output remaining data
    if (nextout > 1) {
      if (nextout > blksize) M.error("nextout too big");
      if (cxin) {
        float outbuf[] = outputBuffer.castF(false);
        // Convert complex array (2d array of floats) into 1D array.
        int count = 0;
        for (int i=1; i<=nextout-1; i++) {
          outbuf[count++] = cbufout[i][REAL];
          outbuf[count++] = cbufout[i][IMAG];
        }
        outputBuffer.uncast(outbuf, true);
      }
      else {
        float outbuf[] = outputBuffer.castF(false);
        int count = 0;
        for (int i=1; i<=nextout-1; i++) {
          outbuf[count++] = bufout[i];
        }
        outputBuffer.uncast(outbuf, true);
      }

      outputFile.write(outputBuffer, nextout-1);
      outputFile.flush();
    }

    // Close all files, post headers, and pass control back to MIDAS
    inputFile.close();
    outputFile.close();

    if (syncout) {
      float syncBuf[] = syncOutBuffer.castF(false);
      int count = 0;
      for (int i=1; i<=npts; i++) {
        syncBuf[count++] = phasebuf[i];
      }
      syncOutBuffer.uncast(syncBuf, true);
      syncOutFile.write(syncOutBuffer, npts);
      syncOutFile.close();
    }


    return NORMAL;
  }

  // Rewrite 4/6/98 dsl
  // time_last: time of the last symbol.
  // t_next : time for the next symbol.
  // tstep : time interval between symbols (instantaneous symbol period).
  // nsyms : number of symbols to be processed with the current value of tstep
  //
  // This version of next_clock does not read data into the phase buffer.
  // This buffer is filled by the calling program.
  private void next_clock (double time_last, double t_next, double tstep, int nsyms) {

    int MAXINT = 2147483647;

    // Make a preliminary estimate of t_next to find sync phase
    t_next = time_last + pri;

    // Test to see if t_next is within the phase buffer
    int bindex = (int) ( (t_next - bufstart) / pp_delta + 1);

    // Test if we are in leadin extrapolation region
    if (bindex < 1) {
      // Recalculate t_next so that it is aligned with symbol clock
      t_next = bufstart - (phasebuf[1] % 1.0) * pri;
      int ioff = (int) (Math.round( (time_last - t_next) / pri));
      t_next = t_next + ioff * pri;
      if (t_next < x_start1) {
        ioff = ((int) ((x_start1 - t_next) / pri)) + 1;
        t_next = t_next + ioff * pri;
      }
      tstep = pri;
      nsyms = (int)((bufstart - t_next) / pri + 1);
    }
    // Test if we are past the end of the phase data
    else if ( bindex >= npts) {

      // We are past end of phase file so extrapolate at nominal rate
      tstep = pri;
      nsyms = MAXINT;
    }
    // Otherwise, we are after first point and before last point so interpolate
    else {
      double phase1 = phasebuf[bindex];
      double time1 = bufstart + (bindex - 1) * pp_delta;
      double phase2 = phasebuf[bindex + 1];
      double pdif = phase2 - phase1;
      pdif = pdif + Math.round( pstep_nominal - pdif);
      double phasint = phase1 + pdif * (t_next - time1) / pp_delta;

      // From the interpolated phase adjust time_next to the nearest symbol.
      t_next = time_last - (phasint - Math.round(phasint) - 1) * pri;
      double tleft = time1 + pp_delta - t_next;

      if (tleft < 0) {
        tleft = tleft + pp_delta;
      }

      tstep = pp_delta / pdif;
      nsyms = (int)(tleft / tstep + 1);
    }

    //Hack to emulate reference params
    //timelast = time_last;
    time_now  = t_next;
    time_step = tstep;
    ncleft    = nsyms;
  }

}
