package nxm.train.prim;

import nxm.sys.inc.*;
import nxm.sys.lib.*;

/** Demodulates an FM signal. */
public final class primlab9 extends Primitive {
  
  private DataFile inFile;        // The input file
  private DataFile outFile;       // The output file
  private Data     inBuf;         // The input data buffer
  private Data     outBuf;        // The output data buffer
  private int      pass;          // iteration through the data
  
  public int open() {
    // Open the input file
    inFile = MA.getDataFile("IN","1000","C#",0);
    inFile.open();
    
    // Open the output file, make sure it is scalar output
    outFile = MA.getDataFile("OUT", inFile, 0);
    outFile.setFormatMode(SCALAR);
    outFile.open();
    
    // Set the transfer length
    xfer = MA.getL("/TL", 4096);
    
    // Allocate input and output buffers of length xfer
    inBuf  = inFile.getDataBuffer(xfer);
    outBuf = outFile.getDataBuffer(xfer);
    
    // set / reset pass 
    pass = 0;   
    
    return NORMAL;
  }

  public int process() {
    int status;
    
    // Read in data
    int numRead = inFile.read(inBuf);
        
    if (numRead == 0) {
      status = NOOP;
    }
    else if (numRead < 0) {
      status = FINISH;
    }
    else {
      status = NORMAL;
      
      // Update pass counter
      pass++;
      
      // "Cast" the data to Java float[] arrays
      float[] in  = inBuf.castF(true);
      float[] out = outBuf.castF(false);
      
      // Process data
      processFM(in, out, numRead);
      
      // "UnCast" the arrays
      inBuf.uncast(in,  false);
      outBuf.uncast(out, true);
      
      // Write data to file
      outFile.write(outBuf, numRead);
    }
    
    return status;
  }

  public int close() {
    // Close the input and output files
    if (inFile  != null) inFile.close();
    if (outFile != null) outFile.close();
    return NORMAL;
  }
  
  //////////////////////////////////////////////////////////////////////////////
  // Code for demodulating the FM signal. You don't need to modify anything below
  // this line.
  //////////////////////////////////////////////////////////////////////////////
  private float valueLast = 0;

  /** Demodulate the data.
      @param fbi Input array.
      @param fbo Output array.
      @param n   Number of elements.
   */
  private void processFM (float[] fbi, float[] fbo, int n) {
    float fr,fi,fp,value,scale=(float)(1.0/TWOPI);
    float bound2=(float)TWOPI, bound=bound2/2;;
    for (int i=0,j=0; i<n; ) {
      fr=fbi[j++]; fi=fbi[j++];
      value = getPhase(fr,fi);
      fp = value-valueLast; 
      // adjust for arctan phase wrap issues by checking for abs(fr) > pi
      if      (fp>=bound) fp -= bound2;
      else if (fp<-bound) fp += bound2;
      fbo[i++] = fp*scale;
      valueLast = value;
    }
    if (pass==1) fbo[0]=fbo[1]; 
  }
  
  /** Gets the phase, only used by processFM(..) */
  private float getPhase (float dr, float di) {
    return (float)Math.atan2((double)di,(double)dr);
  }
}
