package nxm.dsp.prim;

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

/**
  Tracking filter and subharmonic generator <br>
  Note: This class was moved from the UCL option tree in 3.1.0.

  <pre>Port of tracksub primitive from X-Midas.

   NAME: TRACKsub

   PURPOSE:

     Tracking filter and subharmonic generator

   DESCRIPTION:
      Get command line parameters
      Set-up necessary MIDAS files
      Filter and decimate the input data
      Bandpass filter with &lt;filter bandwidth&gt; Hz filter
      Calculate phase of each filtered sample
      Divide phase by &lt; subfactor &gt; and adjust for phase continuity
      at new center frequency by adding i*twopi / &lt; subfactor &gt;
      Convert phase (Mag. = 1) to complex
      Interpolate up to (original sample rate) / &lt; subfactor &gt;
      Write filtered data to output file
      Post headers, close files and return to MIDAS

   CALLING SEQUENCE:

      TRACKsub   &lt; complex input &gt; &lt; carrier output &gt; &lt; fc &gt; &lt; range &gt;
                 &lt; filter bandwidth &gt; &lt; track out &gt; &lt; sub_factor &gt;
                 &lt; samp_factor &gt; &lt; decimated phase &gt;

   INPUT FILES:
      Input file complex carrier harmonic

   OUTPUT FILES:
      Filtered carrier
      Track history or carrier harmonic frequency (optional)
      Decimated phase (optional)

  </pre>

  @author Hoyt J. Heaton
  @author Jason M. Broussard
*/
public class tracksub extends Primitive {

    // -------  CLASS VARIABLES -------------------
  private DataFile inputFile;
  private DataFile outputFile;
  private DataFile freqTrackOutputFile;
  private DataFile decPhaseFile;

  private Data inputData;
  private Data outputData;
  private Data freqTrackOutputData;
  private Data decPhaseData;

  //Constants
  private static final int L_BBUF = 262144;
  private static final int MAXLEN = L_BBUF / 8; //input buffer size
  private static final int LENGTH_MAX = 4219; //filter impulse response length
  private static final int NFILT_MAX = 512;
  private static final int LFILT_MAX = 127;
  private static final int NCOEF = 7;
  private static final float COEF[] = {0, 1.087806f, 1.199617f, 0.461974f, 0.020253f,
      0.001069f, -0.000654f, 0.000232f}; //bandpass filter shape
  private static final float TWOPI = 6.283185308f;
  private static final double DTWOPI = 6.283185307179586476925287;
  private static final int REAL = 0;
  private static final int IMAG = 1;

  //Other class variables
  private boolean trakout;
  private boolean phaseout;
  private int ngot;
  private int start1 = 1;
  private int nleft1;
  private int nget;
  private int incr;
  private int length;
  private int numout;
  private int decfact;
  private float xout;
  private float[][] bpf;
  private float[][][] bpfbank;

  private int nleft2;
  private int istart;
  private float[][] decdata;
  private int lfilt;
  private int nfilt;
  private float gain[];
  private float filtf[];
  private int subfact;
  private float predict_phase;
  private float substep;
  private float phasestep;
  private float gain2[];
  private float trakfreq[];
  private float trakphase[];
  private int lastout;
  private int leninv;
  private float carrdec[][];
  private float carrdecbuf[][];
  private int nleft3;
  private int pstart;
  private int subsamp;
  private int pstart1;
  private float bpfinv[][];
  private int pstart2;
  private int nout;
  private float carrier[][] = new float[MAXLEN][2];
  float[][] c8buf1 = new float[MAXLEN+1][2];
  float[][] c8buf3 = new float[MAXLEN/100][2];

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

  public int open() {

    //Get command-line parameters
    inputFile = MA.getDataFile ("IN", "1000", "CF",0);
    outputFile = MA.getDataFile("OUT", inputFile, 0);
    float fc = (float)MA.getD("FC");
    float range = (float)MA.getD("RANGE");
    double bw = MA.getD("BW");
    subfact = (int)Math.round( MA.getD("SUBFACTOR") );
    subsamp = (int)Math.round( MA.getD("SAMPFACTOR"));

    //initialize variables
    substep = TWOPI / subfact;

    nfilt = Math.max( (int)Math.round(4.0 * range / bw), 1 );
    if (nfilt > NFILT_MAX) {
        M.error("Range exceeds 128*bw");
    }

    inputFile.open();
    int type_code1 = inputFile.getType();
    int x_units1 = inputFile.getXUnits();
    double x_start1 = inputFile.getXStart();
    double x_delta1 = inputFile.getXDelta();
    double size1 = inputFile.getSize();

    //Error checking
    int lhalf = (int)Math.round( 2.107 / (range * x_delta1)  );
    length = lhalf * 2 + 1;
    if (length > LENGTH_MAX) {
        length = LENGTH_MAX;
        decfact = Math.round( LENGTH_MAX / 5.9f);
    }
    else {
        decfact = (int)Math.round( 1.0 / (range * 1.4 * x_delta1));
    }

    //Adjust decfact to be a multiple of subsamp
    decfact = (decfact/2) * 2; // If decfact is odd, move to next lowest even number.
    //int upsamp = decfact / subsamp;  // This line is dead. upsamp is never used.

    pstart = (length - 1) % decfact;
    //int pstart0 = pstart / subsamp;  //Dead-end, pstart0 is never used anywhere.
    pstart1 = pstart + 1;
    pstart2 = pstart + 2;
    predict_phase = 0.0f;
    phasestep = (float)((x_delta1 * fc * substep * decfact) % DTWOPI);

    if (size1 < length) {
        M.error("Data file too short for impulse response file");
    }

    //Determine input transfer length
    xout = (MAXLEN - length + 1.0f) / (float)decfact;
    numout = (int)xout;
    incr = numout * decfact;
    nget = incr + length - 1;
    ngot = nget;
    nleft1 = 0;

    // FILTBANK preamble
    double x_delta3 = x_delta1 * decfact;
    float lowlim = fc - (range/2);
    float hilim = fc + (range/2);
    int ndelay = (int)Math.round( 0.5935 / (x_delta3 * bw)) - 1;
    lfilt = ndelay * 2 + 1;
    if (lfilt > LFILT_MAX) {
        M.error("Bank filters too long");
    }

    //Generate window function for filter
    float window[] = new float[lfilt+1];
    for (int i=1; i<=lfilt; i++) {
        window[i] = (float)Math.cos( (i-ndelay-1)*Math.PI / lfilt );
    }

    //Compute bpf array
    float filtfreq;
    float beta;
    float phase;
    filtf = new float[nfilt+1];
    int tempMax = Math.max(nfilt-1,1);
    int tempLim = lfilt - 1;
    float limDiff = hilim - lowlim;
    float tempProduct = (float)(TWOPI * x_delta3);

    Complex cnum1 = new Complex();
    Complex cnum2 = new Complex();

    bpfbank = new float[nfilt+1][lfilt+1][2];
    for (int i=1; i<=nfilt; i++) {
        filtfreq = lowlim + (i-1) * limDiff / tempMax;
        filtf[i] = filtfreq;
        beta = -filtfreq * tempProduct;

        for (int j=0; j<= tempLim; j++) {
            phase = (j-ndelay) * beta;
            cnum1.real = (float)Math.cos(phase);
            cnum1.imag = (float)Math.sin(phase);
            Complex.mult(cnum1.real, cnum1.imag, window[j+1], cnum1);
            bpfbank[i][j+1][REAL] = cnum1.real;
            bpfbank[i][j+1][IMAG] = cnum1.imag;
        }
    }

    nleft2 = 0;
    istart = 1;
    nleft3 = 0;

    //Adjust header of output file
    outputFile.setXDelta( x_delta1 * subsamp);
    int bias = length - 1 + ndelay * decfact - pstart;
    outputFile.setXStart(x_start1 + x_delta1 * bias );
    outputFile.setSize(( ((int)(size1-length+1)) / decfact + 1 - 2*ndelay - (length-1)/decfact) * decfact/subsamp);
    outputFile.open();

    // Check if either optional output file is needed.
    // If so, set up header info, open file(s)
    String name3 = MA.getS("TRACKOUT");
    String name4 = MA.getS("DECPHASE");
    trakout = !(name3.equals(""));
    phaseout = !(name4.equals(""));

    if (trakout) {
        freqTrackOutputFile = MA.getDataFile("TRACKOUT", "1001","SF", 0);
        freqTrackOutputFile.setXDelta(x_delta3);
        freqTrackOutputFile.setXStart(x_start1 - x_delta1 * (0.5*(length-1) +
            ndelay*decfact));
        freqTrackOutputFile.setXUnits(x_units1);
        freqTrackOutputFile.setSize( ((int)((size1+pstart-length+1)/decfact)) - 2*ndelay );
        freqTrackOutputFile.open();
        freqTrackOutputData = freqTrackOutputFile.getDataBuffer(4096);
    }
    if (phaseout) {
        decPhaseFile = MA.getDataFile("DECPHASE", "1001", "SF", 0);
        decPhaseFile.setXDelta(x_delta3);
        decPhaseFile.setXStart(x_start1 - x_delta1 * (0.5*(length-1) +
            ndelay*decfact));
        decPhaseFile.setXUnits(x_units1);
        decPhaseFile.setSize( ((int)((size1+pstart-length+1)/decfact)) - 2*ndelay );
        decPhaseFile.open();
        decPhaseData = decPhaseFile.getDataBuffer(4096);
    }

    // Make 'range' wide bandpass filter impulse response

    //Generate lowpass prototype for bandpass filter
    //float lengthDiv = length/2.0f - 0.5f;
    float sum, tk;
    float lpf[] = new float[length+1];
    for (int k=1; k<=length; k++) {
        //tk = k - lengthDiv;
        tk = k - length/2.0f - 0.5f;
        sum = 0.0f;
        for (int i=1; i<=NCOEF; i++) {
            sum += COEF[i] * Math.cos( Math.PI * (2 * i - 1) * tk / length);
        }
        lpf[k] = 2 * sum / length;
    }

    //Multiply lowpass by cisoid to compute bandpass
    //bpf = new Complex[LENGTH_MAX];
    bpf = new float[length+1][2];
    beta = (float)(TWOPI * fc * x_delta1);
    for (int i=1; i<=length; i++) {
        phase = (i-1) * beta;
        cnum1.real = (float)Math.cos(phase);
        cnum1.imag = (float)Math.sin(phase);
        Complex.mult(cnum1.real, cnum1.imag, lpf[i], cnum2);
        bpf[i][REAL] = cnum2.real;
        bpf[i][IMAG] = cnum2.imag;
    }

    //Compute gain of bpf filter at each bank filter frequency
    gain = new float[nfilt+1];
    Complex cnumsum = new Complex();
    for (int i=1; i<=nfilt; i++) {
        beta = (float)(filtf[i] * TWOPI * x_delta1);
        cnumsum.real = 0;
        cnumsum.imag = 0;

        for (int j=1; j<= length; j++) {
            phase = -(j-1) * beta;
            cnum1.real = (float)Math.cos(phase);
            cnum1.imag = (float)Math.sin(phase);

            Complex.mult(cnum1.real, cnum1.imag, bpf[j][REAL], bpf[j][IMAG], cnum2);
            Complex.add(cnum2.real, cnum2.imag, cnumsum.real, cnumsum.imag, cnumsum);
        }
        gain[i] = Complex.cabs(cnumsum.real, cnumsum.imag);
    }

    //Compute BPFINV
    bpfinv = new float[length+1][2];
    leninv = length;
    beta = (float)(TWOPI * fc * x_delta1 / subfact);
    int index;
    for (int i=1; i<=leninv; i++) {
        phase = (1-i) * beta;
        index = leninv-i+1;
        cnum1.real = (float)Math.cos(phase);
        cnum1.imag = (float)Math.sin(phase);
        Complex.mult(cnum1.real, cnum1.imag,(decfact * lpf[i]), cnum2);
        bpfinv[index][REAL] = cnum2.real;
        bpfinv[index][IMAG] = cnum2.imag;
    }

    // Compute gain of bpfinv filter at each bank filter frequency
    gain2 = new float[nfilt+1];
    for (int i=1; i<=nfilt; i++) {
        beta = (float)(-filtf[i] * TWOPI * x_delta1 / subfact);
        cnumsum.real = 0; cnumsum.imag = 0;
        for (int j=1; j<=length; j++) {
            phase = (j-1) * beta;
            cnum1.real = (float)Math.cos(phase); cnum1.imag = (float)Math.sin(phase);
            Complex.mult(cnum1.real, cnum1.imag, bpfinv[j][REAL], bpfinv[j][IMAG], cnum2);
            Complex.add(cnumsum.real, cnumsum.imag, cnum2.real, cnum2.imag, cnumsum);
        }
        gain2[i] = (Complex.cabs(cnumsum.real, cnumsum.imag))/decfact;
    }

    inputData = inputFile.getDataBuffer(nget, FLOAT);
    outputData = outputFile.getDataBuffer(MAXLEN);

    decdata = new float[LENGTH_MAX+1][2];
    carrdec = new float[LENGTH_MAX+1][2];
    carrdecbuf = new float[LENGTH_MAX+1][2];

    return NORMAL;
  } // end of open()

    // Process input file until nothing left to read
  public int process() {

    ngot = inputFile.read(inputData, nget);

    if (ngot == 0) { return NOOP;   }
    if (ngot < 0)  { return FINISH; }

    // Process this chunk of input data.
    calc();

    // Prepare various data arrays for output from this iteration.

    //This is only used if a value is provided for the 'track out' parameter
    if (trakout) {
      freqTrackOutputData = freqTrackOutputFile.getDataBuffer(lastout);
      float [] trakbuf = freqTrackOutputData.castF(false);
      int count = 1; //trakfreq is set up as a 1-based array
      for (int i=0; i<lastout; i++ ) {
        trakbuf[i] = trakfreq[count++];
      }
      freqTrackOutputData.uncast(trakbuf, true);
      freqTrackOutputFile.write(freqTrackOutputData);
    }

    //This is only used if a value is provided for the 'decimated phase' parameter
    if (phaseout) {
      decPhaseData = decPhaseFile.getDataBuffer(lastout);
      float [] phasebuf = decPhaseData.castF(false);
      int count = 1; //trakfreq is set up as a 1-based array
      for (int i=0; i<lastout; i++ ) {
        phasebuf[i] = trakphase[count++];
      }
      decPhaseData.uncast(phasebuf, true);
      decPhaseFile.write(decPhaseData);

    }

    // Output data is in the carrier array, which is a 2D array of floats representing an array
    //   of complex numbers.
    // Need to convert this to a serial array of floats for output.
    float outbuf[] = outputData.castF(false);
    int count = 0;
    for (int i=1; i<=nout; i++) {
      outbuf[count++] = carrier[i][REAL];
      outbuf[count++] = carrier[i][IMAG];
    }
    outputData.uncast(outbuf, true);

    outputFile.write(outputData, nout);
    return NORMAL;
  } // end of process()

  public int close() {
    inputFile.close();
    outputFile.close();
    if (trakout) {
      freqTrackOutputFile.close();
    }
    if (phaseout) {
      decPhaseFile.close();
    }
    return NORMAL;
  }

  // Perform calculations
  private void calc() {
    Complex cnum1 = new Complex();

    //Convert series of floats from input file into array of complex numbers.
    float[] inputWork = inputData.castF(true);
    int j = nleft1 + 1;
    try {
      //for (int i=0; i < inputWork.length - 1; i+=2) {
      for (int i=0; i < (ngot*2); i+=2) {
        c8buf1[j][REAL] = inputWork[i];
        c8buf1[j][IMAG] = inputWork[i+1];
        j++;
      }
    }
    catch (ArrayIndexOutOfBoundsException e) {
      System.out.println("j = "+j+" nleft1 = "+nleft1);
      System.exit(0); //TBD -- DO NOT EXIT SYSTEM!
      return;
    }
    inputData.uncast(inputWork, false);

    nleft1 = nleft1 + ngot;

    //Make adjustments so saved points will not be reread.
    if (start1 == 1) {
      start1 = incr + length;
      nget = nget - (length-1);
      ngot = ngot - (length-1);
    }
    else {
      start1 = start1 + incr;
    }

    // How much output will there be after filter & decimate?
    if (ngot != nget) {
      numout = (int) (ngot / (float)decfact );
      if (xout != (int)xout) {
        numout++;
      }
    }

    //Bandpass filter and decimate
    int joff1 = 0;
    Complex cnumsum = new Complex();
    for (int i=1; i<=numout; i++) {
      cnumsum.real = 0; cnumsum.imag = 0;

      for (j=1; j<=length; j++) {
        Complex.mult(c8buf1[joff1 + j][REAL], c8buf1[joff1+j][IMAG],
                bpf[length + 1-j][REAL], bpf[length+1-j][IMAG], cnum1);
        Complex.add(cnumsum.real, cnumsum.imag, cnum1.real, cnum1.imag, cnumsum);
      }
      joff1 += decfact;
      c8buf3[i][REAL] = cnumsum.real; c8buf3[i][IMAG] = cnumsum.imag;
    }

    //Move points to save to start of input buffer
    nleft1 = length - 1;
    for (int i=1; i<=nleft1; i++) {
      c8buf1[i][REAL] = c8buf1[i+incr][REAL];
      c8buf1[i][IMAG] = c8buf1[i+incr][IMAG];
    }

    // FILTBANK processing block
    // Move old points to start of buffer

    for (int i=1; i<=nleft2; i++) {
      decdata[i][REAL] = decdata[i+istart-1][REAL];
      decdata[i][IMAG] = decdata[i+istart-1][IMAG];
    }
    istart = 1;
    for (int i=1; i<=numout; i++) {
      decdata[nleft2+i][REAL] = c8buf3[i][REAL];
      decdata[nleft2+i][IMAG] = c8buf3[i][IMAG];
    }

    nleft2 = nleft2 + numout;
    int nextout = 1;

    // now do the filtering
    float cmag;
    float fsave = 0;
    float cmax;
    int imax = 0;
    float crphase;
    int iadjust;
    Complex csave = new Complex();

    trakfreq = new float[LENGTH_MAX+1];
    trakphase = new float[LENGTH_MAX +1];
    while (nleft2 >= lfilt) {
      cmax = 0;
      for (int i=1; i<=nfilt; i++) {
        cnumsum.real = 0; cnumsum.imag = 0;

        for (j=0; j<= lfilt-1; j++) {
          Complex.mult(decdata[istart+j][REAL], decdata[istart+j][IMAG],
                  bpfbank[i][j+1][REAL], bpfbank[i][j+1][IMAG], cnum1);
          Complex.add(cnumsum.real, cnumsum.imag, cnum1.real, cnum1.imag, cnumsum);
        }

        // compensate for gain of filtdec filter
        Complex.divide(cnumsum.real, cnumsum.imag, gain[i], cnumsum);
        cmag = Complex.cabs(cnumsum.real, cnumsum.imag);
        if (cmag > cmax) {
          cmax = cmag;
          csave.real = cnumsum.real;
          csave.imag = cnumsum.imag;
          fsave = filtf[i];
          imax = i;
        }
      }

      // find output phase as a fraction of filtered phase
      crphase = ((float) (Math.atan2(csave.imag + 1e-30, csave.real))) / subfact;
      iadjust = Math.round( (crphase-predict_phase) / substep );
      crphase = crphase - (iadjust * substep);
      predict_phase = (crphase + phasestep) % TWOPI;

      // precompensate for gain of uplift filter
      if (gain2[imax] == 0) {
          M.error("Zero gain");
      }

      cnum1.real = (float)Math.cos(crphase);
      cnum1.imag = (float)Math.sin(crphase);
      Complex.divide(cnum1.real, cnum1.imag, gain2[imax], cnum1);
      carrdec[nextout][REAL] = cnum1.real;
      carrdec[nextout][IMAG] = cnum1.imag;

      if (trakout) {
        trakfreq[nextout] = fsave;
      }

      if (phaseout) {
        trakphase[nextout] = crphase / TWOPI;
      }

      nextout++;
      nleft2--;
      istart++;
    } //end while (nleft2>=lfilt)

    lastout = nextout - 1; //number of output items to write to trakout and phaseout

    // UPFILT processing
    int L = leninv;
    int ntaps = (length + decfact - 1) / decfact;
    for (int i=1; i<= lastout; i++) {
      carrdecbuf[i+nleft3][REAL] = carrdec[i][REAL];
      carrdecbuf[i+nleft3][IMAG] = carrdec[i][IMAG];
    }

    nleft3 += lastout;
    int ndo = nleft3 - (ntaps - 1);

    if (ndo > 0) {
      for (int i=1; i<=(ndo*decfact+pstart); i++) {
        carrier[i][REAL] = 0;
        carrier[i][IMAG] = 0;
      }
      int kus;
      int jstart;
      int jks;
      for (int k=1; k<=ndo; k++) {
        kus = (k-1) * decfact;
        jstart = (kus % subsamp) + 1;

        for (int i=1; i<=ntaps; i++) {
          jks = (jstart + kus -1) / subsamp + 1;
          for (j=jstart; j<=pstart1; j+=subsamp) {
            int index = L - pstart1 + j - (i-1) * decfact;
            Complex.mult(carrdecbuf[i+(k-1)][REAL], carrdecbuf[i+(k-1)][IMAG],
                    bpfinv[index][REAL], bpfinv[index][IMAG], cnum1);
            Complex.add(carrier[jks][REAL], carrier[jks][IMAG],
                    cnum1.real, cnum1.imag, cnum1);
            carrier[jks][REAL] = cnum1.real; carrier[jks][IMAG] = cnum1.imag;
            jks++;
          }
        }

        jstart = pstart2 + ( (pstart2 + kus - 1) % subsamp);

        for (int i=2; i<=ntaps; i++) {
          jks = (jstart + kus - 1) / subsamp + 1;
          for (j=jstart; j<=decfact; j+=subsamp) {
            int index = L - pstart1 + j - (i-1) * decfact;
            Complex.mult(carrdecbuf[i+(k-1)][REAL], carrdecbuf[i+(k-1)][IMAG],
                    bpfinv[index][REAL], bpfinv[index][IMAG], cnum1);
            Complex.add(carrier[jks][REAL], carrier[jks][IMAG],
                    cnum1.real, cnum1.imag, cnum1);
            carrier[jks][REAL] = cnum1.real; carrier[jks][IMAG] = cnum1.imag;
            jks++;
          }
        }
      }//end for k

      nout = (pstart + nleft3 * decfact - length) / subsamp + 1;
      nleft3 = nleft3 - ndo;

      for (int i=1; i<=nleft3; i++) {
        carrdecbuf[i][REAL] = carrdecbuf[i+ndo][REAL];
        carrdecbuf[i][IMAG] = carrdecbuf[i+ndo][IMAG];
      }
    } //end if (ndo>0)
  } //end of calc()
}
