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

  @author Jeff Schoen
  @version $Id: DemodNative.c,v 1.7 2014/03/26 17:54:45 ntn Exp $
*/

#include "MPULibNative.h"

/* Create Wrappers */
#define CMETHOD(M) Demod_##M
#define JMETHOD(M) Java_nxm_dsp_lib_DemodNative_##M
#include "MPULibWrappers.h"

#define MODELIST "AM,FM,PM,LSB,USB,CW,PSK,QPSK"
#define AM 1
#define FM 2
#define PM 3
#define LSB 4
#define USB 5
#define CW 6
#define PSK 7
#define QPSK 8

#define ALGLIST "STD,LUT"
#define STD 0
#define LUT 1

#define scaleLUT 2147483648.0
#define TW 16
#define TS TW*TW*4
#define CC 0

#define PI 3.141592653589793238462643
#define TWOPI 6.283185307179586476925287

typedef struct {
  int_4 state;
  int_4 mode;
  int_4 diff;
  int_4 algorithm;
  double fullScale;
  int_4 phaseLast;
  int_4 lvalueLast;
  float fvalueLast;
  int_4 pass;
  int_4 tab0[TS],tab1[TS],tab2[TS];
} DemodPlan;

void Demod_initLUT    (DemodPlan *plan);
void Demod_processLUT (DemodPlan *plan, int_2 *bufin, int_2 *bufout, int_4 n);
void Demod_processAM  (DemodPlan *plan, float *bufin, float *bufout, int_4 n);
void Demod_processFM  (DemodPlan *plan, float *bufin, float *bufout, int_4 n);
void Demod_processPM  (DemodPlan *plan, float *bufin, float *bufout, int_4 n);

/*
  MPULib hooks
*/

void* CMETHOD(alloc)() {
  DemodPlan *plan = (DemodPlan*)malloc( sizeof(DemodPlan) );
  plan->state = 0;
  plan->mode = AM;
  plan->diff = 0;
  plan->algorithm = STD;
  plan->phaseLast = 0;
  plan->lvalueLast = 0;
  plan->fvalueLast = 0;
  plan->pass = 0;
  return (void*)plan;
}

int_4 CMETHOD(setkey) (void *handle, char *key, void *data, int_4 len) {
  DemodPlan *plan = (DemodPlan*)handle;
  if      (keyMatch("L:MODULATION")) {
    plan->mode = *(int_4*)data;
    if (plan->mode==FM) plan->diff=1; else plan->diff=0;
    if (plan->algorithm==LUT) Demod_initLUT(plan);
  }
  else if (keyMatch("L:ALGORITHM")) {
    plan->algorithm = *(int_4*)data;
    if (plan->algorithm==LUT) Demod_initLUT(plan);
  }
  else if (keyMatch("D:FULLSCALE")) {
    plan->fullScale = *(double*)data;
  }
  else return 0;
  return len;
}

int_4 CMETHOD(getkey) (void *handle, char *key, void *data, int_4 len) {
  DemodPlan *plan = (DemodPlan*)handle;
  if (keyMatch("L:MODULATION")) { *(int_4*)data = plan->mode; return 4; }
  if (keyMatch("L:ALGORITHM"))  { *(int_4*)data = plan->algorithm; return 4; }
  if (keyMatch("D:FULLSCALE"))  { *(double*)data = plan->fullScale; return 8; }
  if (keyMatch("S:MODULATIONS")){ strcpy (data,MODELIST); return strlen(data); }
  if (keyMatch("S:ALGORITHMS")) { strcpy (data,ALGLIST); return strlen(data); }
  return 0;
}

int_4 CMETHOD(init) (void *handle) {
  DemodPlan *plan = (DemodPlan*)handle;
  if (plan->algorithm==LUT) Demod_initLUT(plan);
  return 0;
}

int_4 CMETHOD(work2) (void *handle, void *buf1, int_4 n1, void *buf2, int_4 n2) {
  DemodPlan *plan = (DemodPlan*)handle;
  if (plan->algorithm==LUT) Demod_processLUT (plan,(int_2*)buf1,(int_2*)buf2,n1);
  else if (plan->mode==AM ) Demod_processAM  (plan,(float*)buf1,(float*)buf2,n1);
  else if (plan->mode==FM ) Demod_processFM  (plan,(float*)buf1,(float*)buf2,n1);
  else if (plan->mode==PM ) Demod_processPM  (plan,(float*)buf1,(float*)buf2,n1);
  else return -1;
  plan->pass++;
  return n1;
}

int_4 CMETHOD(free) (void *handle) {
  free( handle );
  return 0;
}

/*
  Standard Algorithm Code
*/
#define getPhase(R,I) (float)atan2((double)I,(double)R)

float Demod_getPhase (float fr, float fi) {
  return (float)atan2((double)fi,(double)fr);
}

void Demod_processAM (DemodPlan *plan, float *fbi, float *fbo, int_4 n) {
  float fr,fi,value;
  int i,j;
  for (i=0,j=0; i<n; ) {
    fr=fbi[j++]; fi=fbi[j++];
    value = (float)sqrt((double)(fr*fr+fi*fi));
    fbo[i++] = value;
  }
}

void Demod_processFM (DemodPlan *plan, float *fbi, float *fbo, int_4 n) {
  float fr, fi, fp, value, valueLast=plan->fvalueLast, scale=(float)(plan->fullScale/TWOPI);
  float bound2=(float)TWOPI, bound=bound2/2;
  int i,j;
  for (i=0,j=0; i<n; ) {
    fr=fbi[j++]; fi=fbi[j++];
    value = Demod_getPhase(fr,fi);
    fp = value-valueLast;
         if (fp>=bound) fp-=bound2;
    else if (fp<-bound) fp+=bound2;
    fbo[i++] = fp*scale;
    valueLast = value;
  }
  if (plan->pass==0) fbo[0]=fbo[1];
  plan->fvalueLast = valueLast;
}

void Demod_processPM (DemodPlan *plan, float *fbi, float *fbo, int_4 n) {
  float fr, fi, value, scale=(float)(plan->fullScale/TWOPI);
  int i,j;
  for (i=0,j=0; i<n; ) {
    fr=fbi[j++]; fi=fbi[j++];
    fbo[i++] = scale*Demod_getPhase(fr,fi);
  }
}

/*
   LookUpTable Algorithm Code
*/

void Demod_processLUT (DemodPlan *plan, int_2 *ibi, int_2 *ibo, int_4 n) {
  int i,j,k,ar,ai,dr,di,v,vv,vl=plan->lvalueLast;
  int_4 *tab0 = plan->tab0;
  int_4 *tab1 = plan->tab1;
  int_4 *tab2 = plan->tab2;
  for (i=j=0; i<n;) {
    ar = ibi[j++]; ai = ibi[j++];
    k = (ar&0xF800)>>11 | (ai&0xF800)>>6;          // 10 bit lookup table
    dr = ar&0x07FF; di = ai&0x07FF;                // 11 bit residuals
    v = tab0[k] + ((dr*tab1[k] + di*tab2[k])<<CC);
    if (plan->diff) vv = v-vl; else vv=v;
    ibo[i++] = (int_2)(vv>>16);
    vl = v;
  }
  if (plan->diff!=0 && plan->pass==0) ibo[0]=ibo[1];
  plan->lvalueLast = vl;
}

double Demod_lutval (DemodPlan *plan, double imag, double real) {
  double value=0,ar,ai,p;
  if (imag==0 && real==0) return value;
  switch (plan->mode) {
  case AM:
    ai = imag/TW; ar = imag/TW;
    value = sqrt( ar*ar + ai*ai );
    break;
  case FM: case PM:
    p = atan2( imag, real );
    value = (p/PI);
    break;
  }
  return value*scaleLUT;
}

void Demod_initLUT (DemodPlan *plan) {
  int i,j,k=0;
  double h=.5,a=.1,b=.9;
  int_4 *tab0 = plan->tab0;
  int_4 *tab1 = plan->tab1;
  int_4 *tab2 = plan->tab2;
  for (j=-TW; j<TW; j++) {
    for (i=-TW; i<TW; i++) {
      k = (i&0x1F) | ((j&0x1F)<<5);                // 10bit lookup table
      tab0[k] = (int)(Demod_lutval(plan,j+h,i+h));
      tab0[k] = (int)(Demod_lutval(plan,j+h,i+h)/2 +
        Demod_lutval(plan,j+a,i+a)/8 + Demod_lutval(plan,j+b,i+a)/8 +
        Demod_lutval(plan,j+a,i+b)/8 + Demod_lutval(plan,j+b,i+b)/8 );
      tab1[k] = (int)((Demod_lutval(plan,j+h,i+b)-Demod_lutval(plan,j+h,i+a))/(b-a));
      tab2[k] = (int)((Demod_lutval(plan,j+b,i+h)-Demod_lutval(plan,j+a,i+h))/(b-a));
      tab0[k] -= (tab1[k]/2 + tab2[k]/2);
      tab1[k] >>= (11+CC);
      tab2[k] >>= (11+CC);
    }
  }
}
