package nxm.dsp.lib;

import nxm.sys.lib.MPULib;
import nxm.sys.lib.Data;
import nxm.sys.lib.Convert;

/**
  Implements various demodulators on a complex baseband input.

  @author Jeff Schoen
  @author Jason Broussard
*/
public class Demod extends MPULib {

  private int mode,alg,pass;
  private double fullScale;
  private boolean lut,diff,quad;

  /** Modulation Mode List */
  public static final String modeList = "AM,FM,PM,LSB,USB,CW,PSK,QPSK";

  /** Amplitude Modulation Mode                 */ public static final int AM   = 1;
  /** Frequency Modulation Mode                 */ public static final int FM   = 2;
  /** Phase Modulation Mode                     */ public static final int PM   = 3;
  /** Single Sideband Modulation Lower Sideband */ public static final int LSB  = 4;
  /** Single Sideband Modulation Upper Sideband */ public static final int USB  = 5;
  /** Continuous Wave Modulation                */ public static final int CW   = 6;
  /** Phase-Shift Keying Modulation             */ public static final int PSK  = 7;
  /** Quadrature Phase-Shift Keying Modulation  */ public static final int QPSK = 8;

  /** Algorithm List */
  public static final String algList = "STD,LUT";

  /** Standard Algorithm    */ public static final int STD = 1;
  /** LookUpTable Algorithm */ public static final int LUT = 2;

  /** Demod Key List*/
  public static final String keyList = "L:MODULATION,L:ALGORITHM,D:FULLSCALE";

  /** Demod Default Constructor */
  public Demod () {
  }

  /** set parameters */
  public void set (String key, Data value) {
    if (key.equals("MODULATION")) {
      mode = value.toL();
      diff = (mode==FM);
      quad = (mode==FM) || (mode==PM);
      if (lut) initLUT();
    }
    else if (key.equals("FULLSCALE")) {
      fullScale = value.toD();
    }
    else if (key.equals("ALGORITHM")) {
      alg = value.toL();
      lut = (alg==LUT);
      if (lut) initLUT();
    }
    else super.set(key,value);
  }

  /** get parameters */
  public Data get (String key, byte type) {
         if (key.equals("KEYS"))        return new Data(keyList);
    else if (key.equals("MODULATION"))  return new Data(mode);
    else if (key.equals("MODULATIONS")) return new Data(modeList);
    else if (key.equals("FULLSCALE"))   return new Data(fullScale);
    else if (key.equals("ALGORITHM"))   return new Data(alg);
    else if (key.equals("ALGORITHMS"))  return new Data(algList);
    else return super.get(key,type);
  }

  /** Process a buffer of data.
      If LUT, buffers are CI,SI
      else    buffers are CF,SF.
  */
  public int process (byte[] a, int n, byte[] b, int nb) {
    if (lut) {
      short[] ai = Convert.castI(a,true);
      short[] bi = Convert.castI(b,false);
      n = processLUT (ai, bi, n);
      Convert.uncast(ai,a,false);
      Convert.uncast(bi,b,true);
    }
    else {
      float[] af = Convert.castF(a,true);
      float[] bf = Convert.castF(b,false);
      n = processSTD (af, bf, n);
      Convert.uncast(af,a,false);
      Convert.uncast(bf,b,true);
    }
    pass++;
    return n;
  }

  /** Process Demodulation mode with the standard algorithm
   *  @param a input buffer - array of floats
   *  @param b output buffer - array of floats
   *  @param n number elements to process
   *  @return number of data elements processed (or -1 if none)
   */
  public int processSTD (float[] a, float[] b, int n) {
    switch (mode) {
      case AM:  processAM  (a,b,n); break;
      case FM:  processFM  (a,b,n); break;
      case PM:  processPM  (a,b,n); break;
      case PSK: processPSK (a,b,n); break;
      default: n = -1;
    }
    return n;
  }

  private float valueLast;

  private void processAM (float[] fbi, float[] fbo, int n) {
    float fr,fi;
    for (int i=0,j=0; i<n; ) {
      fr=fbi[j++]; fi=fbi[j++];
      fbo[i++] = (float)Math.sqrt((double)(fr*fr+fi*fi));
    }
  }

  private void processFM (float[] fbi, float[] fbo, int n) {
    float fr,fi,fp,value,scale=(float)(fullScale/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==0) fbo[0]=fbo[1];
  }

  private void processPM (float[] fbi, float[] fbo, int n) {
    float fr,fi,scale = (float)(fullScale/TWOPI);
    for (int i=0,j=0; i<n; ) {
      fr=fbi[j++]; fi=fbi[j++];
      fbo[i++] = scale*getPhase(fr,fi);
    }
  }

  private void processPSK (float[] fbi, float[] fbo, int n) {
  }

  private float getPhase (float dr, float di) {
    return (float)Math.atan2((double)di,(double)dr);
  }

  /*
     LookUpTable Java Code Engine
  */

  private static final int DB=16;                               // bits in data type
  private static final int QB=5;                                // addr bits per quadrant
  private static final int TB=QB+1;                             // addr bits total
  private static final int LB=DB-TB;                            // lower address bits
  private static final int QW=1<<QB;                            // divisions per quadrant
  private static final int TW=1<<TB;                            // divisions per table
  private static final int LW=1<<LB;                            // divisions per residual
  private static final int tmask = TW-1;
  private static final int lmask = LW-1;
  private static final int umask = 0xFFFF & ~lmask;

  private int lut0[],lut1[],lut2[],lut3[];
  private int valueLastLUT=0;

  /** Get LUT Frame (divisions per residual)
   *  @return the LUT Frame (divisions per residual)
   */
  public int getLUTframe() { return TW; }

  /** Process Demodulation mode with the LookUpTable
   *  @param ibi input buffer
   *  @param ibo output buffer
   *  @param n   number in first data buffer to process
   *  @return number of data elements processed (or -1 if none)
   */
  public int processLUT (short[] ibi, short[] ibo, int n) {
    int i,j,k,ar,ai,dr,di,v;
    for (i=j=0; i<n; ) {
      ar = ibi[j++]; ai = ibi[j++];
      k = (ai&umask)>>(LB-TB) | (ar&umask)>>LB;         // lookup table
      dr = ar&lmask; di = ai&lmask;                     // residuals
      v = lut0[k] + dr*lut1[k] + di*lut2[k];
      v >>= 15;
      ibo[i++] = (short)(diff? v-valueLastLUT : v);
      valueLastLUT = v;
    }
    if (diff && pass==0) ibo[0]=ibo[1];
    return n;
  }

  private void initLUT() {
    int i,j,k=0;
    double h=.5;
    lut0 = new int[TW*TW];
    lut1 = new int[TW*TW];
    lut2 = new int[TW*TW];
    lut3 = new int[TW*TW];
    for (j=-QW; j<QW; j++) {
      for (i=-QW; i<QW; i++) {
        k = (i&tmask) | ((j&tmask)<<TB);
        lut0[k] = lutval(j+h,i+h);
        lut1[k] = lutval(j+h,i+1)-lutval(j+h,i+0);
        lut2[k] = lutval(j+1,i+h)-lutval(j+0,i+h);
        lut1[k] <<= 1; lut1[k] >>= 1;   // handle wrap
        lut2[k] <<= 1; lut2[k] >>= 1;   // handle wrap
        lut3[k] = lut0[k];
        lut0[k] -= (lut1[k]/2 + lut2[k]/2);
        lut1[k] >>= (16-TB);
        lut2[k] >>= (16-TB);
      }
    }
  }

  private short iut0[],iut1[],iut2[],iut3[];
  private int valueLastFPGA=0;


  /** FPGA uses only 1 quadrant of short LUT values
   *  This code emulates that process (for development) but is slower on CPUs
   *  Quadrant shift: 1=(x,y) 2=(y,-x) 3=(-x,-y) 4=(-y,x)
   *  @param ibi input buffer
   *  @param ibo output buffer
   *  @param n   number elements to process
   *  @return number of data elements processed (or -1 if none)
   */
  public int processFPGA (short[] ibi, short[] ibo, int n) {
    int i,j,k,ar,ai,br,bi,dr,di,v,vv,qa;
    for (i=j=0; i<n;) {
      ar = ibi[j++]; ai = ibi[j++];
      if (ar>0) {
        if (ai>0) { br=ar; bi=ai; qa=0x0000; }
        else      { br=-ai; bi=ar; qa=0xC000; }
      } else {
        if (ai>0) { br=ai; bi=-ar; qa=0x4000; }
        else      { br=-ar; bi=-ai; qa=0x8000; }
      }
      k = (bi&umask)>>(LB-QB) | (br&umask)>>LB;		// lookup table
      dr = br&lmask; di = bi&lmask;			// residuals
      v = iut0[k] + ((dr*iut1[k])>>(16-TB)) + ((di*iut2[k])>>(16-TB));
      if (quad) v = qa | (v>>1); else v <<=1;
      vv = v; if (diff) vv -= valueLastFPGA;
      ibo[i++] = (short)vv;
      valueLastFPGA = v;
    }
    if (diff && pass==0) ibo[0]=ibo[1];
    return n;
  }

  private void initFPGA() {
    int i,j,k,s;
    double h=.5;
    iut0 = new short[QW*QW];
    iut1 = new short[QW*QW];
    iut2 = new short[QW*QW];
    iut3 = new short[QW*QW];
    for (j=0; j<QW; j++) {
      for (i=0; i<QW; i++) {
        k = (i&tmask) | ((j&tmask)<<QB);
        int i0 = lutval(j+h,i+h);
        int i1 = lutval(j+h,i+1)-lutval(j+h,i+0);
        int i2 = lutval(j+1,i+h)-lutval(j+0,i+h);
        int i3 = lutamp(j+h,i+h);
        i1 <<= 1; i1 >>= 1;	// handle wrap
        i2 <<= 1; i2 >>= 1;	// handle wrap
        i0 -= (i1/2 + i2/2);
        s = quad? 14 : 16;	// preserve more precision
        iut0[k] = (short)(i0>>s);
        iut1[k] = (short)(i1>>s);
        iut2[k] = (short)(i2>>s);
        iut3[k] = (short)(i3>>16);
      }
    }
  }

  private static final double scaleAMP = 2147483648.0;
  // allow values > 1 in corners by scaling to 1/2 fullscale and shifting by 1 last
  private static final double scaleLUT = 1073741824.0;

  private int lutval (double imag, double real) {
    double value=0;
    if (imag==0 && real==0) return 0;
    switch (mode) {
    case AM:
      double ai = imag/QW; double ar = real/QW;
      value = Math.sqrt( ar*ar + ai*ai );
      break;
    case FM: case PM:
      double p = Math.atan2( imag, real );
      value = (p/Math.PI);
      break;
    }
    return (int)(value*scaleLUT);
  }

  private int lutamp (double imag, double real) {
    double ai = imag/QW; double ar = real/QW;
    double value = Math.sqrt( ar*ar + ai*ai );
    return (int)Math.min(2147483647.0, value*scaleAMP);
  }

  /** Get LookUpTable entry
   *  @param tn    table number
   *  @param index index of corresponding table
   *  @return LookUpTable entry at index in table tn
   */
  public short getLUTentry (int tn, int index) {
    short[] tab = (tn==1)? iut1 : (tn==2)? iut2 : (tn==3)? iut3 : iut0;
    return tab[index];
  }

  /** Returns whether values are difference to previous value or actual value
   *  @return true for FM
   */
  public boolean isDiff() { return diff; }
  /** Returns whether or not a quadrature Demodulation Mode is being used
   *  @return true if FM or PM
   */
  public boolean isQuad() { return quad; }

}
