#include "dsd.h"

//M17 Filter -- RRC Alpha = 0.5 S/s 48000 with 81 taps
#define M17ZEROS 80
float m17gain = 3.16227766f; //sqrt of 10.0f
static float m17xv[81];

//sample RRC filter for 48kHz sample rate
//alpha=0.5, span=8, sps=10, gain=sqrt(sps)
float m17coeffs[81] = //replaced old ones with ones from libm17
{
	-0.003195702904062073f, -0.002930279157647190f, -0.001940667871554463f,
	-0.000356087678023658f,  0.001547011339077758f,  0.003389554791179751f,
	 0.004761898604225673f,  0.005310860846138910f,  0.004824746306020221f,
	 0.003297923526848786f,  0.000958710871218619f, -0.001749908029791816f,
	-0.004238694106631223f, -0.005881783042101693f, -0.006150256456781309f,
	-0.004745376707651645f, -0.001704189656473565f,  0.002547854551539951f,
	 0.007215575568844704f,  0.011231038205363532f,  0.013421952197060707f,
	 0.012730475385624438f,  0.008449554307303753f,  0.000436744366018287f,
	-0.010735380379191660f, -0.023726883538258272f, -0.036498030780605324f,
	-0.046500883189991064f, -0.050979050575999614f, -0.047340680079891187f,
	-0.033554880492651755f, -0.008513823955725943f,  0.027696543159614194f,
	 0.073664520037517042f,  0.126689053778116234f,  0.182990955139333916f,
	 0.238080025892859704f,  0.287235637987091563f,  0.326040247765297220f,
	 0.350895727088112619f,  0.359452932027607974f,  0.350895727088112619f,
	 0.326040247765297220f,  0.287235637987091563f,  0.238080025892859704f,
	 0.182990955139333916f,  0.126689053778116234f,  0.073664520037517042f,
	 0.027696543159614194f, -0.008513823955725943f, -0.033554880492651755f,
	-0.047340680079891187f, -0.050979050575999614f, -0.046500883189991064f,
	-0.036498030780605324f, -0.023726883538258272f, -0.010735380379191660f,
	 0.000436744366018287f,  0.008449554307303753f,  0.012730475385624438f,
	 0.013421952197060707f,  0.011231038205363532f,  0.007215575568844704f,
	 0.002547854551539951f, -0.001704189656473565f, -0.004745376707651645f,
	-0.006150256456781309f, -0.005881783042101693f, -0.004238694106631223f,
	-0.001749908029791816f,  0.000958710871218619f,  0.003297923526848786f,
	 0.004824746306020221f,  0.005310860846138910f,  0.004761898604225673f,
	 0.003389554791179751f,  0.001547011339077758f, -0.000356087678023658f,
	-0.001940667871554463f, -0.002930279157647190f, -0.003195702904062073f
};

// Original DMR filter
#define NZEROS 60
float ngain = 7.423339364f;
static float xv[NZEROS+1];
float xcoeffs[61] =
{ -0.0083649323f, -0.0265444850f, -0.0428141462f, -0.0537571943f,
-0.0564141052f, -0.0489161045f, -0.0310068662f, -0.0043393881f,
+0.0275375106f, +0.0595423283f, +0.0857543325f, +0.1003565948f,
+0.0986944931f, +0.0782804830f, +0.0395670487f, -0.0136691535f,
-0.0744390415f, -0.1331834575f, -0.1788967208f, -0.2005995448f,
-0.1889627181f, -0.1378439993f, -0.0454976231f, +0.0847488694f,
+0.2444859269f, +0.4209222342f, +0.5982295474f, +0.7593684540f,
+0.8881539892f, +0.9712773915f, +0.9999999166f, +0.9712773915f,
+0.8881539892f, +0.7593684540f, +0.5982295474f, +0.4209222342f,
+0.2444859269f, +0.0847488694f, -0.0454976231f, -0.1378439993f,
-0.1889627181f, -0.2005995448f, -0.1788967208f, -0.1331834575f,
-0.0744390415f, -0.0136691535f, +0.0395670487f, +0.0782804830f,
+0.0986944931f, +0.1003565948f, +0.0857543325f, +0.0595423283f,
+0.0275375106f, -0.0043393881f, -0.0310068662f, -0.0489161045f,
-0.0564141052f, -0.0537571943f, -0.0428141462f, -0.0265444850f,
-0.0083649323f,
};

//reverted to original NXDN filters, do better with LICH/Data handling
// NXDN48 filter - Original Version
#define NXZEROS 134
float nxgain = 15.95930463f;
static float nxv[NXZEROS+1];

float nxcoeffs[135] =
{ +0.031462429f, +0.031747267f, +0.030401148f, +0.027362877f,
+0.022653298f, +0.016379869f, +0.008737200f, +0.000003302f,
-0.009468531f, -0.019262057f, -0.028914291f, -0.037935027f,
-0.045828927f, -0.052119261f, -0.056372283f, -0.058221106f,
-0.057387924f, -0.053703443f, -0.047122444f, -0.037734535f,
-0.025769308f, -0.011595336f, +0.004287292f, +0.021260954f,
+0.038610717f, +0.055550276f, +0.071252765f, +0.084885375f,
+0.095646450f, +0.102803611f, +0.105731303f, +0.103946126f,
+0.097138329f, +0.085197939f, +0.068234131f, +0.046586711f,
+0.020828821f, -0.008239664f, -0.039608255f, -0.072081234f,
-0.104311776f, -0.134843790f, -0.162160200f, -0.184736015f,
-0.201094346f, -0.209863285f, -0.209831516f, -0.200000470f,
-0.179630919f, -0.148282051f, -0.105841323f, -0.052543664f,
+0.011020985f, +0.083912428f, +0.164857408f, +0.252278939f,
+0.344336996f, +0.438979335f, +0.534000832f, +0.627109358f,
+0.715995947f, +0.798406824f, +0.872214756f, +0.935487176f,
+0.986548646f, +1.024035395f, +1.046939951f, +1.054644241f,
+1.046939951f, +1.024035395f, +0.986548646f, +0.935487176f,
+0.872214756f, +0.798406824f, +0.715995947f, +0.627109358f,
+0.534000832f, +0.438979335f, +0.344336996f, +0.252278939f,
+0.164857408f, +0.083912428f, +0.011020985f, -0.052543664f,
-0.105841323f, -0.148282051f, -0.179630919f, -0.200000470f,
-0.209831516f, -0.209863285f, -0.201094346f, -0.184736015f,
-0.162160200f, -0.134843790f, -0.104311776f, -0.072081234f,
-0.039608255f, -0.008239664f, +0.020828821f, +0.046586711f,
+0.068234131f, +0.085197939f, +0.097138329f, +0.103946126f,
+0.105731303f, +0.102803611f, +0.095646450f, +0.084885375f,
+0.071252765f, +0.055550276f, +0.038610717f, +0.021260954f,
+0.004287292f, -0.011595336f, -0.025769308f, -0.037734535f,
-0.047122444f, -0.053703443f, -0.057387924f, -0.058221106f,
-0.056372283f, -0.052119261f, -0.045828927f, -0.037935027f,
-0.028914291f, -0.019262057f, -0.009468531f, +0.000003302f,
+0.008737200f, +0.016379869f, +0.022653298f, +0.027362877f,
+0.030401148f, +0.031747267f, +0.031462429f,
};

// DMR filter F4EXB
#define DMRNZEROS 60

// DMR filter F4EXB
const float dmrgain = 6.82973073748f;

// DMR filter F4EXB
static float dxv[DMRNZEROS+1];

// DMR filter F4EXB - root raised cosine alpha=0.7 Ts = 6650 S/s Fc = 48kHz
const float dmrcoeffs[61] =
{0.0301506278, 0.0269200615, 0.0159662432, -0.0013114705, -0.0216605133,
-0.0404938748, -0.0528141756, -0.0543747957, -0.0428325003, -0.0186176083,
0.0147202645, 0.0508418571, 0.0816392577, 0.0988113688, 0.0957187780,
0.0691512084, 0.0206194642, -0.0431564563, -0.1107569268, -0.1675773224,
-0.1981519842, -0.1889130786, -0.1308939560, -0.0218608492, 0.1325685970,
0.3190962499, 0.5182530574, 0.7070497652, 0.8623526878, 0.9644213921,
1.0000000000, 0.9644213921, 0.8623526878, 0.7070497652, 0.5182530574,
0.3190962499, 0.1325685970, -0.0218608492, -0.1308939560, -0.1889130786,
-0.1981519842, -0.1675773224, -0.1107569268, -0.0431564563, 0.0206194642,
0.0691512084, 0.0957187780, 0.0988113688, 0.0816392577, 0.0508418571,
0.0147202645, -0.0186176083, -0.0428325003, -0.0543747957, -0.0528141756,
-0.0404938748, -0.0216605133, -0.0013114705, 0.0159662432, 0.0269200615, 0.0301506278};

// dPMR filter, version of F4EXB
static float dpmrxv[NXZEROS+1];

// dPMR filter, version of F4EXB
#define DPMRNZEROS 134

// dPMR filter, version of F4EXB
const float dpmrgain = 14.6083498224f;

// dPMR filter - root raised cosine alpha=0.2 Ts = 3325 S/s Fc = 48 kHz - zero at boundaries appears to be slightly better
const float dpmrcoeffs[135] =
{-0.0000983004, 0.0058388841,  0.0119748846,  0.0179185547, 0.0232592816,
 0.0275919612,  0.0305433586,  0.0317982965,  0.0311240307,
 0.0283911865,  0.0235897433,  0.0168387650,  0.0083888763,
-0.0013831396, -0.0119878087, -0.0228442151, -0.0333082708,
-0.0427067804, -0.0503756642, -0.0557003599, -0.0581561791,
-0.0573462646, -0.0530347941, -0.0451732069, -0.0339174991,
-0.0196350217, -0.0028997157,  0.0155246961,  0.0347134030,
 0.0536202583,  0.0711271166,  0.0861006725,  0.0974542022,
 0.1042112035,  0.1055676660,  0.1009496091,  0.0900625944,
 0.0729301774,  0.0499186839,  0.0217462748, -0.0105250265,
-0.0455148664, -0.0815673067, -0.1168095612, -0.1492246435,
-0.1767350726, -0.1972941202, -0.2089805758, -0.2100926829,
-0.1992367833, -0.1754063031, -0.1380470370, -0.0871052089,
-0.0230554989,  0.0530929052,  0.1398131936,  0.2351006721,
 0.3365341927,  0.4413570929,  0.5465745033,  0.6490630781,
 0.7456885564,  0.8334261381,  0.9094784589,  0.9713859928,
 1.0171250045,  1.0451886943,  1.0546479089,  1.0451886943,
 1.0171250045,  0.9713859928,  0.9094784589,  0.8334261381,
 0.7456885564,  0.6490630781,  0.5465745033,  0.4413570929,
 0.3365341927,  0.2351006721,  0.1398131936,  0.0530929052,
-0.0230554989, -0.0871052089, -0.1380470370, -0.1754063031,
-0.1992367833, -0.2100926829, -0.2089805758, -0.1972941202,
-0.1767350726, -0.1492246435, -0.1168095612, -0.0815673067,
-0.0455148664, -0.0105250265,  0.0217462748,  0.0499186839,
 0.0729301774,  0.0900625944,  0.1009496091,  0.1055676660,
 0.1042112035,  0.0974542022,  0.0861006725,  0.0711271166,
 0.0536202583,  0.0347134030,  0.0155246961, -0.0028997157,
-0.0196350217, -0.0339174991, -0.0451732069, -0.0530347941,
-0.0573462646, -0.0581561791, -0.0557003599, -0.0503756642,
-0.0427067804, -0.0333082708, -0.0228442151, -0.0119878087,
-0.0013831396,  0.0083888763,  0.0168387650,  0.0235897433,
 0.0283911865,  0.0311240307,  0.0317982965,  0.0305433586,
 0.0275919612,  0.0232592816,  0.0179185547,  0.0119748846,
 0.0058388841, -0.0000983004};



short dsd_input_filter(short sample, int mode);

short
dmr_filter(short sample)
{
    return dsd_input_filter(sample, 3);
}

short
nxdn_filter(short sample)
{
    return dsd_input_filter(sample, 2);
}

short
dpmr_filter(short sample)
{
    return dsd_input_filter(sample, 4);
}

short m17_filter (short sample)
{
  return dsd_input_filter(sample, 5);
}


short
dsd_input_filter(short sample, int mode)
{
  float sum; int i;
  float gain;
  int zeros;
  float *v, *coeffs;
  switch(mode) {
    case 1:
      gain = ngain;
      v = xv;
      coeffs = xcoeffs;
      zeros = NZEROS;
      break;
    case 2:
      gain = nxgain;
      v = nxv;
      coeffs = (float *)nxcoeffs;
      zeros = NXZEROS;
      break;
    case 3:
        gain = dmrgain;
        v = dxv;
        coeffs = (float *)dmrcoeffs;
        zeros = DMRNZEROS;
        break;
    case 4:
        gain = dpmrgain;
        v = dpmrxv;
        coeffs = (float *)dpmrcoeffs;
        zeros = DPMRNZEROS;
        break;

    case 5:
      gain = m17gain;
      v = m17xv;
      coeffs = (float *)m17coeffs;
      zeros = M17ZEROS;
      break;

    default:
      return sample;
  }

  for (i = 0; i < zeros; i++)
      v[i] = v[i+1];

  v[zeros] = (float)sample; // unfiltered sample in
  sum = 0.0f;

  for (i = 0; i <= zeros; i++)
    sum += (coeffs[i] * v[i]);

  return (short)(sum / gain); // filtered sample out
}

void init_rrc_filter_memory()
{
  memset (m17xv, 0, 81*sizeof(float));
  memset (xv, 0, 61*sizeof(float));
  memset (dxv, 0, 61*sizeof(float));
  memset (nxv, 0, 135*sizeof(float));
  memset (dpmrxv, 0, 135*sizeof(float));
}