<commit_before>/*
* Copyright 2008, 2011 Free Software Foundation, Inc.
*
* This software is distributed under the terms of the GNU Affero Public License.
* See the COPYING file in the main directory for details.
*
* This use of this software may be subject to additional restrictions.
* See the LEGAL file in the main directory for details.

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU Affero General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU Affero General Public License for more details.

	You should have received a copy of the GNU Affero General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/



#define NDEBUG

#include "sigProcLib.h"
#include "GSMCommon.h"
#include "sendLPF_961.h"
#include "rcvLPF_651.h"

#include <Logger.h>

#define TABLESIZE 1024

/** Lookup tables for trigonometric approximation */
float cosTable[TABLESIZE+1]; // add 1 element for wrap around
float sinTable[TABLESIZE+1];

/** Constants */
static const float M_PI_F = (float)M_PI;
static const float M_2PI_F = (float)(2.0*M_PI);
static const float M_1_2PI_F = 1/M_2PI_F;

/** Static vectors that contain a precomputed +/- f_b/4 sinusoid */ 
signalVector *GMSKRotation = NULL;
signalVector *GMSKReverseRotation = NULL;

/** Static ideal RACH and midamble correlation waveforms */
typedef struct {
  signalVector *sequence;
  signalVector *sequenceReversedConjugated;
  float        TOA;
  complex      gain;
} CorrelationSequence;

CorrelationSequence *gMidambles[] = {NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL};
CorrelationSequence *gRACHSequence = NULL;

void sigProcLibDestroy(void) {
  if (GMSKRotation) {
    delete GMSKRotation;
    GMSKRotation = NULL;
  }
  if (GMSKReverseRotation) {
    delete GMSKReverseRotation;
    GMSKReverseRotation = NULL;
  }
  for (int i = 0; i < 8; i++) {
    if (gMidambles[i]!=NULL) {
      if (gMidambles[i]->sequence) delete gMidambles[i]->sequence;
      if (gMidambles[i]->sequenceReversedConjugated) delete gMidambles[i]->sequenceReversedConjugated;
      delete gMidambles[i];
      gMidambles[i] = NULL;
    }
  }
  if (gRACHSequence) {
    if (gRACHSequence->sequence) delete gRACHSequence->sequence;
    if (gRACHSequence->sequenceReversedConjugated) delete gRACHSequence->sequenceReversedConjugated;
    delete gRACHSequence;
    gRACHSequence = NULL;
  }
}



// dB relative to 1.0.
// if > 1.0, then return 0 dB
float dB(float x) {
  
  float arg = 1.0F;
  float dB = 0.0F;
  
  if (x >= 1.0F) return 0.0F;
  if (x <= 0.0F) return -200.0F;

  float prevArg = arg;
  float prevdB = dB;
  float stepSize = 16.0F;
  float dBstepSize = 12.0F;
  while (stepSize > 1.0F) {
    do {
      prevArg = arg;
      prevdB = dB;
      arg /= stepSize;
      dB -= dBstepSize;
    } while (arg > x);
    arg = prevArg;
    dB = prevdB;
    stepSize *= 0.5F;
    dBstepSize -= 3.0F;
  }
 return ((arg-x)*(dB-3.0F) + (x-arg*0.5F)*dB)/(arg - arg*0.5F);

}

// 10^(-dB/10), inverse of dB func.
float dBinv(float x) {
  
  float arg = 1.0F;
  float dB = 0.0F;
  
  if (x >= 0.0F) return 1.0F;
  if (x <= -200.0F) return 0.0F;

  float prevArg = arg;
  float prevdB = dB;
  float stepSize = 16.0F;
  float dBstepSize = 12.0F;
  while (stepSize > 1.0F) {
    do {
      prevArg = arg;
      prevdB = dB;
      arg /= stepSize;
      dB -= dBstepSize;
    } while (dB > x);
    arg = prevArg;
    dB = prevdB;
    stepSize *= 0.5F;
    dBstepSize -= 3.0F;
  }

  return ((dB-x)*(arg*0.5F)+(x-(dB-3.0F))*(arg))/3.0F;

}

float vectorNorm2(const signalVector &x) 
{
  signalVector::const_iterator xPtr = x.begin();
  float Energy = 0.0;
  for (;xPtr != x.end();xPtr++) {
	Energy += xPtr->norm2();
  }
  return Energy;
}


float vectorPower(const signalVector &x) 
{
  return vectorNorm2(x)/x.size();
}

/** compute cosine via lookup table */
float cosLookup(const float x)
{
  float arg = x*M_1_2PI_F;
  while (arg > 1.0F) arg -= 1.0F;
  while (arg < 0.0F) arg += 1.0F;

  const float argT = arg*((float)TABLESIZE);
  const int argI = (int)argT;
  const float delta = argT-argI;
  const float iDelta = 1.0F-delta;
  return iDelta*cosTable[argI] + delta*cosTable[argI+1];
}

/** compute sine via lookup table */
float sinLookup(const float x) 
{
  float arg = x*M_1_2PI_F;
  while (arg > 1.0F) arg -= 1.0F;
  while (arg < 0.0F) arg += 1.0F;

  const float argT = arg*((float)TABLESIZE);
  const int argI = (int)argT;
  const float delta = argT-argI;
  const float iDelta = 1.0F-delta;
  return iDelta*sinTable[argI] + delta*sinTable[argI+1];
}


/** compute e^(-jx) via lookup table. */
complex expjLookup(float x)
{
  float arg = x*M_1_2PI_F;
  while (arg > 1.0F) arg -= 1.0F;
  while (arg < 0.0F) arg += 1.0F;

  const float argT = arg*((float)TABLESIZE);
  const int argI = (int)argT;
  const float delta = argT-argI;
  const float iDelta = 1.0F-delta;
   return complex(iDelta*cosTable[argI] + delta*cosTable[argI+1],
		   iDelta*sinTable[argI] + delta*sinTable[argI+1]);
}

/** Library setup functions */
void initTrigTables() {
  for (int i = 0; i < TABLESIZE+1; i++) {
    cosTable[i] = cos(2.0*M_PI*i/TABLESIZE);
    sinTable[i] = sin(2.0*M_PI*i/TABLESIZE);
  }
}

void initGMSKRotationTables(int samplesPerSymbol) {
  GMSKRotation = new signalVector(157*samplesPerSymbol);
  GMSKReverseRotation = new signalVector(157*samplesPerSymbol);
  signalVector::iterator rotPtr = GMSKRotation->begin();
  signalVector::iterator revPtr = GMSKReverseRotation->begin();
  float phase = 0.0;
  while (rotPtr != GMSKRotation->end()) {
    *rotPtr++ = expjLookup(phase);
    *revPtr++ = expjLookup(-phase);
    phase += M_PI_F/2.0F/(float) samplesPerSymbol;
  }
}

void sigProcLibSetup(int samplesPerSymbol) {
  initTrigTables();
  initGMSKRotationTables(samplesPerSymbol);
}

void GMSKRotate(signalVector &x) {
  signalVector::iterator xPtr = x.begin();
  signalVector::iterator rotPtr = GMSKRotation->begin();
  if (x.isRealOnly()) {
    while (xPtr < x.end()) {
      *xPtr = *rotPtr++ * (xPtr->real());
      xPtr++;
    }
  }
  else {
    while (xPtr < x.end()) {
      *xPtr = *rotPtr++ * (*xPtr);
      xPtr++;
    }
  }
}

void GMSKReverseRotate(signalVector &x) {
  signalVector::iterator xPtr= x.begin();
  signalVector::iterator rotPtr = GMSKReverseRotation->begin();
  if (x.isRealOnly()) {
    while (xPtr < x.end()) {
      *xPtr = *rotPtr++ * (xPtr->real());
      xPtr++;
    }
  }
  else {
    while (xPtr < x.end()) {
      *xPtr = *rotPtr++ * (*xPtr);
      xPtr++;
    }
  }
}


signalVector* convolve(const signalVector *a,
		       const signalVector *b,
		       signalVector *c,
		       ConvType spanType,
		       unsigned startIx,
		       unsigned len)
{
  if ((a==NULL) || (b==NULL)) return NULL; 
  int La = a->size();
  int Lb = b->size();

  int startIndex;
  unsigned int outSize;
  switch (spanType) {
    case FULL_SPAN:
      startIndex = 0;
      outSize = La+Lb-1;
      break;
    case OVERLAP_ONLY:
      startIndex = La;
      outSize = abs(La-Lb)+1;
      break;
    case START_ONLY:
      startIndex = 0;
      outSize = La;
      break;
    case WITH_TAIL:
      startIndex = Lb;
      outSize = La;
      break;
    case NO_DELAY:
      if (Lb % 2) 
	startIndex = Lb/2;
      else
	startIndex = Lb/2-1;
      outSize = La;
      break;
    case CUSTOM:
      startIndex = startIx;
      outSize = len;
      break;
    default:
      return NULL;
  }

  
  if (c==NULL)
    c = new signalVector(outSize);
  else if (c->size()!=outSize)
    return NULL;

  signalVector::const_iterator aStart = a->begin();
  signalVector::const_iterator bStart = b->begin();
  signalVector::const_iterator aEnd = a->end();
  signalVector::const_iterator bEnd = b->end();
  signalVector::iterator cPtr = c->begin();
  int t = startIndex;
  int stopIndex = startIndex + outSize;
  switch (b->getSymmetry()) {
  case NONE:
    {
      while (t < stopIndex) {
	signalVector::const_iterator aP = aStart+t;
	signalVector::const_iterator bP = bStart;
	if (a->isRealOnly() && b->isRealOnly()) {
	  float sum = 0.0;
	  while (bP < bEnd) {
	    if (aP < aStart) break;
	    if (aP < aEnd) sum += (aP->real())*(bP->real());
	    aP--;
	    bP++;
	  }
	  *cPtr++ = sum;
	}
	else if (a->isRealOnly()) {
	  complex sum = 0.0;
	  while (bP < bEnd) {
	    if (aP < aStart) break;
	    if (aP < aEnd) sum += (*bP)*(aP->real());
	    aP--;
	    bP++;
	  }
	  *cPtr++ = sum;
	}
	else if (b->isRealOnly()) {
	  complex sum = 0.0;
	  while (bP < bEnd) {
	    if (aP < aStart) break;
	    if (aP < aEnd) sum += (*aP)*(bP->real());
	    aP--;
	    bP++;
	  }
	  *cPtr++ = sum;
	}
	else {
	  complex sum = 0.0;
	  while (bP < bEnd) {
	    if (aP < aStart) break;
	    if (aP < aEnd) sum += (*aP)*(*bP);
	    aP--;
	    bP++;
	  }
	  *cPtr++ = sum;
	}
	t++;
      }
    }
    break;
  case ABSSYM:
    {
      complex sum = 0.0;
      bool isOdd = (bool) (Lb % 2);
      if (isOdd) 
	bEnd = bStart + (Lb+1)/2;
      else 
	bEnd = bStart + Lb/2;
      while (t < stopIndex) {
	signalVector::const_iterator aP = aStart+t;
	signalVector::const_iterator aPsym = aP-Lb+1;
	signalVector::const_iterator bP = bStart;
	sum = 0.0;
        if (!b->isRealOnly()) {
	  while (bP < bEnd) {
	    if (aP < aStart) break;
	    if (aP == aPsym)
	      sum+= (*aP)*(*bP);
	    else if ((aP < aEnd) && (aPsym >= aStart)) 
	      sum+= ((*aP)+(*aPsym))*(*bP);
	    else if (aP < aEnd)
	      sum += (*aP)*(*bP);
	    else if (aPsym >= aStart)
	      sum += (*aPsym)*(*bP);
	    aP--;
	    aPsym++;
	    bP++;
	  }
        }
        else {
          while (bP < bEnd) {
            if (aP < aStart) break;
            if (aP == aPsym)
              sum+= (*aP)*(bP->real());
            else if ((aP < aEnd) && (aPsym >= aStart))
              sum+= ((*aP)+(*aPsym))*(bP->real());
            else if (aP < aEnd)
              sum += (*aP)*(bP->real());
            else if (aPsym >= aStart)
              sum += (*aPsym)*(bP->real());
            aP--;
            aPsym++;
            bP++;
          }
        }
	*cPtr++ = sum;
	t++;
      }
    }
    break;
  default:
    return NULL;
    break;
  }
    
    
  return c;
}


signalVector* generateGSMPulse(int symbolLength,
			       int samplesPerSymbol)
{

  int numSamples = samplesPerSymbol*symbolLength + 1;
  signalVector *x = new signalVector(numSamples);
  signalVector::iterator xP = x->begin();
  int centerPoint = (numSamples-1)/2;
  for (int i = 0; i < numSamples; i++) {
    float arg = (float) (i-centerPoint)/(float) samplesPerSymbol;
    *xP++ = 0.96*exp(-1.1380*arg*arg-0.527*arg*arg*arg*arg); // GSM pulse approx.
  }

  float avgAbsval = sqrtf(vectorNorm2(*x)/samplesPerSymbol);
  xP = x->begin();
  for (int i = 0; i < numSamples; i++) 
    *xP++ /= avgAbsval;
  x->isRealOnly(true);
  x->setSymmetry(ABSSYM);
  return x;
}

signalVector* frequencyShift(signalVector *y,
			     signalVector *x,
			     float freq,
			     float startPhase,
			     float *finalPhase)
{

  if (!x) return NULL;
 
  if (y==NULL) {
    y = new signalVector(x->size());
    y->isRealOnly(x->isRealOnly());
    if (y==NULL) return NULL;
  }

  if (y->size() < x->size()) return NULL;

  float phase = startPhase;
  signalVector::iterator yP = y->begin();
  signalVector::iterator xPEnd = x->end();
  signalVector::iterator xP = x->begin();

  if (x->isRealOnly()) {
    while (xP < xPEnd) {
      (*yP++) = expjLookup(phase)*( (xP++)->real() );
      phase += freq;
    }
  }
  else {
    while (xP < xPEnd) {
      (*yP++) = (*xP++)*expjLookup(phase);
      phase += freq;
    }
  }


  if (finalPhase) *finalPhase = phase;

  return y;
}

signalVector* reverseConjugate(signalVector *b)
{
    signalVector *tmp = new signalVector(b->size());
    tmp->isRealOnly(b->isRealOnly());
    signalVector::iterator bP = b->begin();
    signalVector::iterator bPEnd = b->end();
    signalVector::iterator tmpP = tmp->end()-1;
    if (!b->isRealOnly()) {
      while (bP < bPEnd) {
        *tmpP-- = bP->conj();
        bP++;
      }
    }
    else {
      while (bP < bPEnd) {
        *tmpP-- = bP->real();
        bP++;
      }
    }

    return tmp;
}

signalVector* correlate(signalVector *a,
			signalVector *b,
			signalVector *c,
			ConvType spanType,
			bool bReversedConjugated,
		        unsigned startIx,
			unsigned len)
{
  signalVector *tmp = NULL;

  if (!bReversedConjugated) {
    tmp = reverseConjugate(b);
  }
  else {
    tmp = b;
  }

  c = convolve(a,tmp,c,spanType,startIx,len);

  if (!bReversedConjugated) delete tmp;

  return c;
}


/* soft output slicer */
bool vectorSlicer(signalVector *x) 
{

  signalVector::iterator xP = x->begin();
  signalVector::iterator xPEnd = x->end();
  while (xP < xPEnd) {
    *xP = (complex) (0.5*(xP->real()+1.0F));
    if (xP->real() > 1.0) *xP = 1.0;
    if (xP->real() < 0.0) *xP = 0.0;
    xP++;
  }
  return true;
}
  
signalVector *modulateBurst(const BitVector &wBurst,
			    const signalVector &gsmPulse,
			    int guardPeriodLength,
			    int samplesPerSymbol)
{

  //static complex staticBurst[157];

  int burstSize = samplesPerSymbol*(wBurst.size()+guardPeriodLength);
  //signalVector modBurst((complex *) staticBurst,0,burstSize);
  signalVector modBurst(burstSize);// = new signalVector(burstSize);
  modBurst.isRealOnly(true);
  //memset(staticBurst,0,sizeof(complex)*burstSize);
  modBurst.fill(0.0);
  signalVector::iterator modBurstItr = modBurst.begin();

#if 0 
  // if wBurst is already differentially decoded
  *modBurstItr = 2.0*(wBurst[0] & 0x01)-1.0;
  signalVector::iterator prevVal = modBurstItr;
  for (unsigned int i = 1; i < wBurst.size(); i++) {
    modBurstItr += samplesPerSymbol;
    if (wBurst[i] & 0x01) 
      *modBurstItr = *prevVal * complex(0.0,1.0);
    else
      *modBurstItr = *prevVal * complex(0.0,-1.0);
    prevVal = modBurstItr;
  }
#else
  // if wBurst are the raw bits
  for (unsigned int i = 0; i < wBurst.size(); i++) {
    *modBurstItr = 2.0*(wBurst[i] & 0x01)-1.0;
    modBurstItr += samplesPerSymbol;
  }

  // shift up pi/2
  // ignore starting phase, since spec allows for discontinuous phase
  GMSKRotate(modBurst);
#endif
  modBurst.isRealOnly(false);

  // filter w/ pulse shape
  signalVector *shapedBurst = convolve(&modBurst,&gsmPulse,NULL,NO_DELAY);

  //delete modBurst;
  
  return shapedBurst;

}

float sinc(float x) 
{
  if ((x >= 0.01F) || (x <= -0.01F)) return (sinLookup(x)/x);
  return 1.0F;
}

void delayVector(signalVector &wBurst,
		 float delay)
{
  
  int   intOffset = (int) floor(delay);
  float fracOffset = delay - intOffset;
  
  // do fractional shift first, only do it for reasonable offsets
  if (fabs(fracOffset) > 1e-2) {
    // create sinc function
    signalVector sincVector(21); 
    sincVector.isRealOnly(true);
    signalVector::iterator sincBurstItr = sincVector.begin();
    for (int i = 0; i < 21; i++) 
      *sincBurstItr++ = (complex) sinc(M_PI_F*(i-10-fracOffset));
  
    signalVector shiftedBurst(wBurst.size());
    convolve(&wBurst,&sincVector,&shiftedBurst,NO_DELAY);
    wBurst.clone(shiftedBurst);
  }

  if (intOffset < 0) {
    intOffset = -intOffset;
    signalVector::iterator wBurstItr = wBurst.begin();
    signalVector::iterator shiftedItr = wBurst.begin()+intOffset;
    while (shiftedItr < wBurst.end())
      *wBurstItr++ = *shiftedItr++;
    while (wBurstItr < wBurst.end())
      *wBurstItr++ = 0.0;
  }
  else {
    signalVector::iterator wBurstItr = wBurst.end()-1;
    signalVector::iterator shiftedItr = wBurst.end()-1-intOffset;
    while (shiftedItr >= wBurst.begin())
      *wBurstItr-- = *shiftedItr--;
    while (wBurstItr >= wBurst.begin())
      *wBurstItr-- = 0.0;
  }
}
  
signalVector *gaussianNoise(int length, 
			    float variance, 
			    complex mean)
{

  signalVector *noise = new signalVector(length);
  signalVector::iterator nPtr = noise->begin();
  float stddev = sqrtf(variance);
  while (nPtr < noise->end()) {
    float u1 = (float) rand()/ (float) RAND_MAX;
    while (u1==0.0)
      u1 = (float) rand()/ (float) RAND_MAX;
    float u2 = (float) rand()/ (float) RAND_MAX;
    float arg = 2.0*M_PI*u2;
    *nPtr = mean + stddev*complex(cos(arg),sin(arg))*sqrtf(-2.0*log(u1));
    nPtr++;
  }

  return noise;
}

complex interpolatePoint(const signalVector &inSig,
			 float ix)
{
  
  int start = (int) (floor(ix) - 10);
  if (start < 0) start = 0;
  int end = (int) (floor(ix) + 11);
  if ((unsigned) end > inSig.size()-1) end = inSig.size()-1;
  
  complex pVal = 0.0;
  if (!inSig.isRealOnly()) {
    for (int i = start; i < end; i++) 
      pVal += inSig[i] * sinc(M_PI_F*(i-ix));
  }
  else {
    for (int i = start; i < end; i++) 
      pVal += inSig[i].real() * sinc(M_PI_F*(i-ix));
  }
   
  return pVal;
}

  
 
complex peakDetect(const signalVector &rxBurst,
		   float *peakIndex,
		   float *avgPwr) 
{
  

  complex maxVal = 0.0;
  float maxIndex = -1;
  float sumPower = 0.0;

  for (unsigned int i = 0; i < rxBurst.size(); i++) {
    float samplePower = rxBurst[i].norm2();
    if (samplePower > maxVal.real()) {
      maxVal = samplePower;
      maxIndex = i;
    }
    sumPower += samplePower;
  }

  // interpolate around the peak
  // to save computation, we'll use early-late balancing
  float earlyIndex = maxIndex-1;
  float lateIndex = maxIndex+1;
  
  float incr = 0.5;
  while (incr > 1.0/1024.0) {
    complex earlyP = interpolatePoint(rxBurst,earlyIndex);
    complex lateP =  interpolatePoint(rxBurst,lateIndex);
    if (earlyP < lateP) 
      earlyIndex += incr;
    else if (earlyP > lateP)
      earlyIndex -= incr;
    else break;
    incr /= 2.0;
    lateIndex = earlyIndex + 2.0;
  }

  maxIndex = earlyIndex + 1.0;
  maxVal = interpolatePoint(rxBurst,maxIndex);

  if (peakIndex!=NULL)
    *peakIndex = maxIndex;

  if (avgPwr!=NULL)
    *avgPwr = (sumPower-maxVal.norm2()) / (rxBurst.size()-1);

  return maxVal;

}

void scaleVector(signalVector &x,
		 complex scale)
{
  signalVector::iterator xP = x.begin();
  signalVector::iterator xPEnd = x.end();
  if (!x.isRealOnly()) {
    while (xP < xPEnd) {
      *xP = *xP * scale;
      xP++;
    }
  }
  else {
    while (xP < xPEnd) {
      *xP = xP->real() * scale;
      xP++;
    }
  }
}

/** in-place conjugation */
void conjugateVector(signalVector &x)
{
  if (x.isRealOnly()) return;
  signalVector::iterator xP = x.begin();
  signalVector::iterator xPEnd = x.end();
  while (xP < xPEnd) {
    *xP = xP->conj();
    xP++;
  }
}


// in-place addition!!
bool addVector(signalVector &x,
	       signalVector &y)
{
  signalVector::iterator xP = x.begin();
  signalVector::iterator yP = y.begin();
  signalVector::iterator xPEnd = x.end();
  signalVector::iterator yPEnd = y.end();
  while ((xP < xPEnd) && (yP < yPEnd)) {
    *xP = *xP + *yP;
    xP++; yP++;
  }
  return true;
}

// in-place multiplication!!
bool multVector(signalVector &x,
                 signalVector &y)
{
  signalVector::iterator xP = x.begin();
  signalVector::iterator yP = y.begin();
  signalVector::iterator xPEnd = x.end();
  signalVector::iterator yPEnd = y.end();
  while ((xP < xPEnd) && (yP < yPEnd)) {
    *xP = (*xP) * (*yP);
    xP++; yP++;
  }
  return true;
}


void offsetVector(signalVector &x,
		  complex offset)
{
  signalVector::iterator xP = x.begin();
  signalVector::iterator xPEnd = x.end();
  if (!x.isRealOnly()) {
    while (xP < xPEnd) {
      *xP += offset;
      xP++;
    }
  }
  else {
    while (xP < xPEnd) {
      *xP = xP->real() + offset;
      xP++;
    }      
  }
}

bool generateMidamble(signalVector &gsmPulse,
		      int samplesPerSymbol,
		      int TSC)
{

  if ((TSC < 0) || (TSC > 7)) 
    return false;

  if (gMidambles[TSC]) {
    if (gMidambles[TSC]->sequence!=NULL) delete gMidambles[TSC]->sequence;
    if (gMidambles[TSC]->sequenceReversedConjugated!=NULL)  delete gMidambles[TSC]->sequenceReversedConjugated;
  }

  signalVector emptyPulse(1); 
  *(emptyPulse.begin()) = 1.0;

  // only use middle 16 bits of each TSC
  signalVector *middleMidamble = modulateBurst(gTrainingSequence[TSC].segment(5,16),
					 emptyPulse,
					 0,
					 samplesPerSymbol);
  signalVector *midamble = modulateBurst(gTrainingSequence[TSC],
                                         gsmPulse,
                                         0,
                                         samplesPerSymbol);
  
  if (midamble == NULL) return false;
  if (middleMidamble == NULL) return false;

  // NOTE: Because ideal TSC 16-bit midamble is 66 symbols into burst,
  //       the ideal TSC has an + 180 degree phase shift,
  //       due to the pi/2 frequency shift, that 
  //       needs to be accounted for.
  //       26-midamble is 61 symbols into burst, has +90 degree phase shift.
  scaleVector(*middleMidamble,complex(-1.0,0.0));
  scaleVector(*midamble,complex(0.0,1.0));

  signalVector *autocorr = correlate(midamble,middleMidamble,NULL,NO_DELAY);
  
  if (autocorr == NULL) return false;

  gMidambles[TSC] = new CorrelationSequence;
  gMidambles[TSC]->sequence = middleMidamble;
  gMidambles[TSC]->sequenceReversedConjugated = reverseConjugate(middleMidamble);
  gMidambles[TSC]->gain = peakDetect(*autocorr,&gMidambles[TSC]->TOA,NULL);

  LOG(DEBUG) << "midamble autocorr: " << *autocorr;

  LOG(DEBUG) << "TOA: " << gMidambles[TSC]->TOA;

  //gMidambles[TSC]->TOA -= 5*samplesPerSymbol;

  delete autocorr;
  delete midamble;

  return true;
}

bool generateRACHSequence(signalVector &gsmPulse,
			  int samplesPerSymbol)
{
  
  if (gRACHSequence) {
    if (gRACHSequence->sequence!=NULL) delete gRACHSequence->sequence;
    if (gRACHSequence->sequenceReversedConjugated!=NULL) delete gRACHSequence->sequenceReversedConjugated;
  }

  signalVector *RACHSeq = modulateBurst(gRACHSynchSequence,
					gsmPulse,
					0,
					samplesPerSymbol);

  assert(RACHSeq);

  signalVector *autocorr = correlate(RACHSeq,RACHSeq,NULL,NO_DELAY);

  assert(autocorr);

  gRACHSequence = new CorrelationSequence;
  gRACHSequence->sequence = RACHSeq;
  gRACHSequence->sequenceReversedConjugated = reverseConjugate(RACHSeq);
  gRACHSequence->gain = peakDetect(*autocorr,&gRACHSequence->TOA,NULL);
 
  delete autocorr;

  return true;

}

				
bool detectRACHBurst(signalVector &rxBurst,
		     float detectThreshold,
		     int samplesPerSymbol,
		     complex *amplitude,
		     float* TOA)
{

  //static complex staticData[500];
 
  //signalVector correlatedRACH(staticData,0,rxBurst.size());
  signalVector correlatedRACH(rxBurst.size());
  correlate(&rxBurst,gRACHSequence->sequenceReversedConjugated,&correlatedRACH,NO_DELAY,true);

  float meanPower;
  complex peakAmpl = peakDetect(correlatedRACH,TOA,&meanPower);

  float valleyPower = 0.0; 

  // check for bogus results
  if ((*TOA < 0.0) || (*TOA > correlatedRACH.size())) {
        *amplitude = 0.0;
	return false;
  }
  complex *peakPtr = correlatedRACH.begin() + (int) rint(*TOA);

  LOG(DEBUG) << "RACH corr: " << correlatedRACH;

  float numSamples = 0.0;
  for (int i = 57*samplesPerSymbol; i <= 107*samplesPerSymbol;i++) {
    if (peakPtr+i >= correlatedRACH.end())
      break;
    valleyPower += (peakPtr+i)->norm2();
    numSamples++;
  }

  if (numSamples < 2) {
        *amplitude = 0.0;
        return false;
  }

  float RMS = sqrtf(valleyPower/(float) numSamples)+0.00001;
  float peakToMean = peakAmpl.abs()/RMS;

  LOG(DEBUG) << "RACH peakAmpl=" << peakAmpl << " RMS=" << RMS << " peakToMean=" << peakToMean;
  *amplitude = peakAmpl/(gRACHSequence->gain);

  *TOA = (*TOA) - gRACHSequence->TOA - 8*samplesPerSymbol;

  LOG(DEBUG) << "RACH thresh: " << peakToMean;

  return (peakToMean > detectThreshold);
}

bool energyDetect(signalVector &rxBurst,
		  unsigned windowLength,
		  float detectThreshold,
                  float *avgPwr)
{

  signalVector::const_iterator windowItr = rxBurst.begin(); //+rxBurst.size()/2 - 5*windowLength/2;
  float energy = 0.0;
  if (windowLength < 0) windowLength = 20;
  if (windowLength > rxBurst.size()) windowLength = rxBurst.size();
  for (unsigned i = 0; i < windowLength; i++) {
    energy += windowItr->norm2();
    windowItr+=4;
  }
  if (avgPwr) *avgPwr = energy/windowLength;
  LOG(DEBUG) << "detected energy: " << energy/windowLength;
  return (energy/windowLength > detectThreshold*detectThreshold);
}
  

bool analyzeTrafficBurst(signalVector &rxBurst,
			 unsigned TSC,
			 float detectThreshold,
			 int samplesPerSymbol,
			 complex *amplitude,
			 float *TOA,
			 unsigned maxTOA,
                         bool requestChannel,
                         signalVector **channelResponse,
			 float *channelResponseOffset) 
{

  assert(TSC<8);
  assert(amplitude);
  assert(TOA);
  assert(gMidambles[TSC]);

  if (maxTOA < 3*samplesPerSymbol) maxTOA = 3*samplesPerSymbol;
  unsigned spanTOA = maxTOA;
  if (spanTOA < 5*samplesPerSymbol) spanTOA = 5*samplesPerSymbol;

  unsigned startIx = (66-spanTOA)*samplesPerSymbol;
  unsigned endIx = (66+16+spanTOA)*samplesPerSymbol;
  unsigned windowLen = endIx - startIx;
  unsigned corrLen = 2*maxTOA+1;

  unsigned expectedTOAPeak = (unsigned) round(gMidambles[TSC]->TOA + (gMidambles[TSC]->sequenceReversedConjugated->size()-1)/2);

  signalVector burstSegment(rxBurst.begin(),startIx,windowLen);

  //static complex staticData[200];
  //signalVector correlatedBurst(staticData,0,corrLen);
  signalVector correlatedBurst(corrLen);
  correlate(&burstSegment, gMidambles[TSC]->sequenceReversedConjugated,
					    &correlatedBurst, CUSTOM,true,
					    expectedTOAPeak-maxTOA,corrLen);

  float meanPower;<commit_msg>Transceiver52M: Fix calculation of TS correlation for 2 sps and higher<commit_after>/*
* Copyright 2008, 2011 Free Software Foundation, Inc.
*
* This software is distributed under the terms of the GNU Affero Public License.
* See the COPYING file in the main directory for details.
*
* This use of this software may be subject to additional restrictions.
* See the LEGAL file in the main directory for details.

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU Affero General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU Affero General Public License for more details.

	You should have received a copy of the GNU Affero General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/



#define NDEBUG

#include "sigProcLib.h"
#include "GSMCommon.h"
#include "sendLPF_961.h"
#include "rcvLPF_651.h"

#include <Logger.h>

#define TABLESIZE 1024

/** Lookup tables for trigonometric approximation */
float cosTable[TABLESIZE+1]; // add 1 element for wrap around
float sinTable[TABLESIZE+1];

/** Constants */
static const float M_PI_F = (float)M_PI;
static const float M_2PI_F = (float)(2.0*M_PI);
static const float M_1_2PI_F = 1/M_2PI_F;

/** Static vectors that contain a precomputed +/- f_b/4 sinusoid */ 
signalVector *GMSKRotation = NULL;
signalVector *GMSKReverseRotation = NULL;

/** Static ideal RACH and midamble correlation waveforms */
typedef struct {
  signalVector *sequence;
  signalVector *sequenceReversedConjugated;
  float        TOA;
  complex      gain;
} CorrelationSequence;

CorrelationSequence *gMidambles[] = {NULL,NULL,NULL,NULL,NULL,NULL,NULL,NULL};
CorrelationSequence *gRACHSequence = NULL;

void sigProcLibDestroy(void) {
  if (GMSKRotation) {
    delete GMSKRotation;
    GMSKRotation = NULL;
  }
  if (GMSKReverseRotation) {
    delete GMSKReverseRotation;
    GMSKReverseRotation = NULL;
  }
  for (int i = 0; i < 8; i++) {
    if (gMidambles[i]!=NULL) {
      if (gMidambles[i]->sequence) delete gMidambles[i]->sequence;
      if (gMidambles[i]->sequenceReversedConjugated) delete gMidambles[i]->sequenceReversedConjugated;
      delete gMidambles[i];
      gMidambles[i] = NULL;
    }
  }
  if (gRACHSequence) {
    if (gRACHSequence->sequence) delete gRACHSequence->sequence;
    if (gRACHSequence->sequenceReversedConjugated) delete gRACHSequence->sequenceReversedConjugated;
    delete gRACHSequence;
    gRACHSequence = NULL;
  }
}



// dB relative to 1.0.
// if > 1.0, then return 0 dB
float dB(float x) {
  
  float arg = 1.0F;
  float dB = 0.0F;
  
  if (x >= 1.0F) return 0.0F;
  if (x <= 0.0F) return -200.0F;

  float prevArg = arg;
  float prevdB = dB;
  float stepSize = 16.0F;
  float dBstepSize = 12.0F;
  while (stepSize > 1.0F) {
    do {
      prevArg = arg;
      prevdB = dB;
      arg /= stepSize;
      dB -= dBstepSize;
    } while (arg > x);
    arg = prevArg;
    dB = prevdB;
    stepSize *= 0.5F;
    dBstepSize -= 3.0F;
  }
 return ((arg-x)*(dB-3.0F) + (x-arg*0.5F)*dB)/(arg - arg*0.5F);

}

// 10^(-dB/10), inverse of dB func.
float dBinv(float x) {
  
  float arg = 1.0F;
  float dB = 0.0F;
  
  if (x >= 0.0F) return 1.0F;
  if (x <= -200.0F) return 0.0F;

  float prevArg = arg;
  float prevdB = dB;
  float stepSize = 16.0F;
  float dBstepSize = 12.0F;
  while (stepSize > 1.0F) {
    do {
      prevArg = arg;
      prevdB = dB;
      arg /= stepSize;
      dB -= dBstepSize;
    } while (dB > x);
    arg = prevArg;
    dB = prevdB;
    stepSize *= 0.5F;
    dBstepSize -= 3.0F;
  }

  return ((dB-x)*(arg*0.5F)+(x-(dB-3.0F))*(arg))/3.0F;

}

float vectorNorm2(const signalVector &x) 
{
  signalVector::const_iterator xPtr = x.begin();
  float Energy = 0.0;
  for (;xPtr != x.end();xPtr++) {
	Energy += xPtr->norm2();
  }
  return Energy;
}


float vectorPower(const signalVector &x) 
{
  return vectorNorm2(x)/x.size();
}

/** compute cosine via lookup table */
float cosLookup(const float x)
{
  float arg = x*M_1_2PI_F;
  while (arg > 1.0F) arg -= 1.0F;
  while (arg < 0.0F) arg += 1.0F;

  const float argT = arg*((float)TABLESIZE);
  const int argI = (int)argT;
  const float delta = argT-argI;
  const float iDelta = 1.0F-delta;
  return iDelta*cosTable[argI] + delta*cosTable[argI+1];
}

/** compute sine via lookup table */
float sinLookup(const float x) 
{
  float arg = x*M_1_2PI_F;
  while (arg > 1.0F) arg -= 1.0F;
  while (arg < 0.0F) arg += 1.0F;

  const float argT = arg*((float)TABLESIZE);
  const int argI = (int)argT;
  const float delta = argT-argI;
  const float iDelta = 1.0F-delta;
  return iDelta*sinTable[argI] + delta*sinTable[argI+1];
}


/** compute e^(-jx) via lookup table. */
complex expjLookup(float x)
{
  float arg = x*M_1_2PI_F;
  while (arg > 1.0F) arg -= 1.0F;
  while (arg < 0.0F) arg += 1.0F;

  const float argT = arg*((float)TABLESIZE);
  const int argI = (int)argT;
  const float delta = argT-argI;
  const float iDelta = 1.0F-delta;
   return complex(iDelta*cosTable[argI] + delta*cosTable[argI+1],
		   iDelta*sinTable[argI] + delta*sinTable[argI+1]);
}

/** Library setup functions */
void initTrigTables() {
  for (int i = 0; i < TABLESIZE+1; i++) {
    cosTable[i] = cos(2.0*M_PI*i/TABLESIZE);
    sinTable[i] = sin(2.0*M_PI*i/TABLESIZE);
  }
}

void initGMSKRotationTables(int samplesPerSymbol) {
  GMSKRotation = new signalVector(157*samplesPerSymbol);
  GMSKReverseRotation = new signalVector(157*samplesPerSymbol);
  signalVector::iterator rotPtr = GMSKRotation->begin();
  signalVector::iterator revPtr = GMSKReverseRotation->begin();
  float phase = 0.0;
  while (rotPtr != GMSKRotation->end()) {
    *rotPtr++ = expjLookup(phase);
    *revPtr++ = expjLookup(-phase);
    phase += M_PI_F/2.0F/(float) samplesPerSymbol;
  }
}

void sigProcLibSetup(int samplesPerSymbol) {
  initTrigTables();
  initGMSKRotationTables(samplesPerSymbol);
}

void GMSKRotate(signalVector &x) {
  signalVector::iterator xPtr = x.begin();
  signalVector::iterator rotPtr = GMSKRotation->begin();
  if (x.isRealOnly()) {
    while (xPtr < x.end()) {
      *xPtr = *rotPtr++ * (xPtr->real());
      xPtr++;
    }
  }
  else {
    while (xPtr < x.end()) {
      *xPtr = *rotPtr++ * (*xPtr);
      xPtr++;
    }
  }
}

void GMSKReverseRotate(signalVector &x) {
  signalVector::iterator xPtr= x.begin();
  signalVector::iterator rotPtr = GMSKReverseRotation->begin();
  if (x.isRealOnly()) {
    while (xPtr < x.end()) {
      *xPtr = *rotPtr++ * (xPtr->real());
      xPtr++;
    }
  }
  else {
    while (xPtr < x.end()) {
      *xPtr = *rotPtr++ * (*xPtr);
      xPtr++;
    }
  }
}


signalVector* convolve(const signalVector *a,
		       const signalVector *b,
		       signalVector *c,
		       ConvType spanType,
		       unsigned startIx,
		       unsigned len)
{
  if ((a==NULL) || (b==NULL)) return NULL; 
  int La = a->size();
  int Lb = b->size();

  int startIndex;
  unsigned int outSize;
  switch (spanType) {
    case FULL_SPAN:
      startIndex = 0;
      outSize = La+Lb-1;
      break;
    case OVERLAP_ONLY:
      startIndex = La;
      outSize = abs(La-Lb)+1;
      break;
    case START_ONLY:
      startIndex = 0;
      outSize = La;
      break;
    case WITH_TAIL:
      startIndex = Lb;
      outSize = La;
      break;
    case NO_DELAY:
      if (Lb % 2) 
	startIndex = Lb/2;
      else
	startIndex = Lb/2-1;
      outSize = La;
      break;
    case CUSTOM:
      startIndex = startIx;
      outSize = len;
      break;
    default:
      return NULL;
  }

  
  if (c==NULL)
    c = new signalVector(outSize);
  else if (c->size()!=outSize)
    return NULL;

  signalVector::const_iterator aStart = a->begin();
  signalVector::const_iterator bStart = b->begin();
  signalVector::const_iterator aEnd = a->end();
  signalVector::const_iterator bEnd = b->end();
  signalVector::iterator cPtr = c->begin();
  int t = startIndex;
  int stopIndex = startIndex + outSize;
  switch (b->getSymmetry()) {
  case NONE:
    {
      while (t < stopIndex) {
	signalVector::const_iterator aP = aStart+t;
	signalVector::const_iterator bP = bStart;
	if (a->isRealOnly() && b->isRealOnly()) {
	  float sum = 0.0;
	  while (bP < bEnd) {
	    if (aP < aStart) break;
	    if (aP < aEnd) sum += (aP->real())*(bP->real());
	    aP--;
	    bP++;
	  }
	  *cPtr++ = sum;
	}
	else if (a->isRealOnly()) {
	  complex sum = 0.0;
	  while (bP < bEnd) {
	    if (aP < aStart) break;
	    if (aP < aEnd) sum += (*bP)*(aP->real());
	    aP--;
	    bP++;
	  }
	  *cPtr++ = sum;
	}
	else if (b->isRealOnly()) {
	  complex sum = 0.0;
	  while (bP < bEnd) {
	    if (aP < aStart) break;
	    if (aP < aEnd) sum += (*aP)*(bP->real());
	    aP--;
	    bP++;
	  }
	  *cPtr++ = sum;
	}
	else {
	  complex sum = 0.0;
	  while (bP < bEnd) {
	    if (aP < aStart) break;
	    if (aP < aEnd) sum += (*aP)*(*bP);
	    aP--;
	    bP++;
	  }
	  *cPtr++ = sum;
	}
	t++;
      }
    }
    break;
  case ABSSYM:
    {
      complex sum = 0.0;
      bool isOdd = (bool) (Lb % 2);
      if (isOdd) 
	bEnd = bStart + (Lb+1)/2;
      else 
	bEnd = bStart + Lb/2;
      while (t < stopIndex) {
	signalVector::const_iterator aP = aStart+t;
	signalVector::const_iterator aPsym = aP-Lb+1;
	signalVector::const_iterator bP = bStart;
	sum = 0.0;
        if (!b->isRealOnly()) {
	  while (bP < bEnd) {
	    if (aP < aStart) break;
	    if (aP == aPsym)
	      sum+= (*aP)*(*bP);
	    else if ((aP < aEnd) && (aPsym >= aStart)) 
	      sum+= ((*aP)+(*aPsym))*(*bP);
	    else if (aP < aEnd)
	      sum += (*aP)*(*bP);
	    else if (aPsym >= aStart)
	      sum += (*aPsym)*(*bP);
	    aP--;
	    aPsym++;
	    bP++;
	  }
        }
        else {
          while (bP < bEnd) {
            if (aP < aStart) break;
            if (aP == aPsym)
              sum+= (*aP)*(bP->real());
            else if ((aP < aEnd) && (aPsym >= aStart))
              sum+= ((*aP)+(*aPsym))*(bP->real());
            else if (aP < aEnd)
              sum += (*aP)*(bP->real());
            else if (aPsym >= aStart)
              sum += (*aPsym)*(bP->real());
            aP--;
            aPsym++;
            bP++;
          }
        }
	*cPtr++ = sum;
	t++;
      }
    }
    break;
  default:
    return NULL;
    break;
  }
    
    
  return c;
}


signalVector* generateGSMPulse(int symbolLength,
			       int samplesPerSymbol)
{

  int numSamples = samplesPerSymbol*symbolLength + 1;
  signalVector *x = new signalVector(numSamples);
  signalVector::iterator xP = x->begin();
  int centerPoint = (numSamples-1)/2;
  for (int i = 0; i < numSamples; i++) {
    float arg = (float) (i-centerPoint)/(float) samplesPerSymbol;
    *xP++ = 0.96*exp(-1.1380*arg*arg-0.527*arg*arg*arg*arg); // GSM pulse approx.
  }

  float avgAbsval = sqrtf(vectorNorm2(*x)/samplesPerSymbol);
  xP = x->begin();
  for (int i = 0; i < numSamples; i++) 
    *xP++ /= avgAbsval;
  x->isRealOnly(true);
  x->setSymmetry(ABSSYM);
  return x;
}

signalVector* frequencyShift(signalVector *y,
			     signalVector *x,
			     float freq,
			     float startPhase,
			     float *finalPhase)
{

  if (!x) return NULL;
 
  if (y==NULL) {
    y = new signalVector(x->size());
    y->isRealOnly(x->isRealOnly());
    if (y==NULL) return NULL;
  }

  if (y->size() < x->size()) return NULL;

  float phase = startPhase;
  signalVector::iterator yP = y->begin();
  signalVector::iterator xPEnd = x->end();
  signalVector::iterator xP = x->begin();

  if (x->isRealOnly()) {
    while (xP < xPEnd) {
      (*yP++) = expjLookup(phase)*( (xP++)->real() );
      phase += freq;
    }
  }
  else {
    while (xP < xPEnd) {
      (*yP++) = (*xP++)*expjLookup(phase);
      phase += freq;
    }
  }


  if (finalPhase) *finalPhase = phase;

  return y;
}

signalVector* reverseConjugate(signalVector *b)
{
    signalVector *tmp = new signalVector(b->size());
    tmp->isRealOnly(b->isRealOnly());
    signalVector::iterator bP = b->begin();
    signalVector::iterator bPEnd = b->end();
    signalVector::iterator tmpP = tmp->end()-1;
    if (!b->isRealOnly()) {
      while (bP < bPEnd) {
        *tmpP-- = bP->conj();
        bP++;
      }
    }
    else {
      while (bP < bPEnd) {
        *tmpP-- = bP->real();
        bP++;
      }
    }

    return tmp;
}

signalVector* correlate(signalVector *a,
			signalVector *b,
			signalVector *c,
			ConvType spanType,
			bool bReversedConjugated,
		        unsigned startIx,
			unsigned len)
{
  signalVector *tmp = NULL;

  if (!bReversedConjugated) {
    tmp = reverseConjugate(b);
  }
  else {
    tmp = b;
  }

  c = convolve(a,tmp,c,spanType,startIx,len);

  if (!bReversedConjugated) delete tmp;

  return c;
}


/* soft output slicer */
bool vectorSlicer(signalVector *x) 
{

  signalVector::iterator xP = x->begin();
  signalVector::iterator xPEnd = x->end();
  while (xP < xPEnd) {
    *xP = (complex) (0.5*(xP->real()+1.0F));
    if (xP->real() > 1.0) *xP = 1.0;
    if (xP->real() < 0.0) *xP = 0.0;
    xP++;
  }
  return true;
}
  
signalVector *modulateBurst(const BitVector &wBurst,
			    const signalVector &gsmPulse,
			    int guardPeriodLength,
			    int samplesPerSymbol)
{

  //static complex staticBurst[157];

  int burstSize = samplesPerSymbol*(wBurst.size()+guardPeriodLength);
  //signalVector modBurst((complex *) staticBurst,0,burstSize);
  signalVector modBurst(burstSize);// = new signalVector(burstSize);
  modBurst.isRealOnly(true);
  //memset(staticBurst,0,sizeof(complex)*burstSize);
  modBurst.fill(0.0);
  signalVector::iterator modBurstItr = modBurst.begin();

#if 0 
  // if wBurst is already differentially decoded
  *modBurstItr = 2.0*(wBurst[0] & 0x01)-1.0;
  signalVector::iterator prevVal = modBurstItr;
  for (unsigned int i = 1; i < wBurst.size(); i++) {
    modBurstItr += samplesPerSymbol;
    if (wBurst[i] & 0x01) 
      *modBurstItr = *prevVal * complex(0.0,1.0);
    else
      *modBurstItr = *prevVal * complex(0.0,-1.0);
    prevVal = modBurstItr;
  }
#else
  // if wBurst are the raw bits
  for (unsigned int i = 0; i < wBurst.size(); i++) {
    *modBurstItr = 2.0*(wBurst[i] & 0x01)-1.0;
    modBurstItr += samplesPerSymbol;
  }

  // shift up pi/2
  // ignore starting phase, since spec allows for discontinuous phase
  GMSKRotate(modBurst);
#endif
  modBurst.isRealOnly(false);

  // filter w/ pulse shape
  signalVector *shapedBurst = convolve(&modBurst,&gsmPulse,NULL,NO_DELAY);

  //delete modBurst;
  
  return shapedBurst;

}

float sinc(float x) 
{
  if ((x >= 0.01F) || (x <= -0.01F)) return (sinLookup(x)/x);
  return 1.0F;
}

void delayVector(signalVector &wBurst,
		 float delay)
{
  
  int   intOffset = (int) floor(delay);
  float fracOffset = delay - intOffset;
  
  // do fractional shift first, only do it for reasonable offsets
  if (fabs(fracOffset) > 1e-2) {
    // create sinc function
    signalVector sincVector(21); 
    sincVector.isRealOnly(true);
    signalVector::iterator sincBurstItr = sincVector.begin();
    for (int i = 0; i < 21; i++) 
      *sincBurstItr++ = (complex) sinc(M_PI_F*(i-10-fracOffset));
  
    signalVector shiftedBurst(wBurst.size());
    convolve(&wBurst,&sincVector,&shiftedBurst,NO_DELAY);
    wBurst.clone(shiftedBurst);
  }

  if (intOffset < 0) {
    intOffset = -intOffset;
    signalVector::iterator wBurstItr = wBurst.begin();
    signalVector::iterator shiftedItr = wBurst.begin()+intOffset;
    while (shiftedItr < wBurst.end())
      *wBurstItr++ = *shiftedItr++;
    while (wBurstItr < wBurst.end())
      *wBurstItr++ = 0.0;
  }
  else {
    signalVector::iterator wBurstItr = wBurst.end()-1;
    signalVector::iterator shiftedItr = wBurst.end()-1-intOffset;
    while (shiftedItr >= wBurst.begin())
      *wBurstItr-- = *shiftedItr--;
    while (wBurstItr >= wBurst.begin())
      *wBurstItr-- = 0.0;
  }
}
  
signalVector *gaussianNoise(int length, 
			    float variance, 
			    complex mean)
{

  signalVector *noise = new signalVector(length);
  signalVector::iterator nPtr = noise->begin();
  float stddev = sqrtf(variance);
  while (nPtr < noise->end()) {
    float u1 = (float) rand()/ (float) RAND_MAX;
    while (u1==0.0)
      u1 = (float) rand()/ (float) RAND_MAX;
    float u2 = (float) rand()/ (float) RAND_MAX;
    float arg = 2.0*M_PI*u2;
    *nPtr = mean + stddev*complex(cos(arg),sin(arg))*sqrtf(-2.0*log(u1));
    nPtr++;
  }

  return noise;
}

complex interpolatePoint(const signalVector &inSig,
			 float ix)
{
  
  int start = (int) (floor(ix) - 10);
  if (start < 0) start = 0;
  int end = (int) (floor(ix) + 11);
  if ((unsigned) end > inSig.size()-1) end = inSig.size()-1;
  
  complex pVal = 0.0;
  if (!inSig.isRealOnly()) {
    for (int i = start; i < end; i++) 
      pVal += inSig[i] * sinc(M_PI_F*(i-ix));
  }
  else {
    for (int i = start; i < end; i++) 
      pVal += inSig[i].real() * sinc(M_PI_F*(i-ix));
  }
   
  return pVal;
}

  
 
complex peakDetect(const signalVector &rxBurst,
		   float *peakIndex,
		   float *avgPwr) 
{
  

  complex maxVal = 0.0;
  float maxIndex = -1;
  float sumPower = 0.0;

  for (unsigned int i = 0; i < rxBurst.size(); i++) {
    float samplePower = rxBurst[i].norm2();
    if (samplePower > maxVal.real()) {
      maxVal = samplePower;
      maxIndex = i;
    }
    sumPower += samplePower;
  }

  // interpolate around the peak
  // to save computation, we'll use early-late balancing
  float earlyIndex = maxIndex-1;
  float lateIndex = maxIndex+1;
  
  float incr = 0.5;
  while (incr > 1.0/1024.0) {
    complex earlyP = interpolatePoint(rxBurst,earlyIndex);
    complex lateP =  interpolatePoint(rxBurst,lateIndex);
    if (earlyP < lateP) 
      earlyIndex += incr;
    else if (earlyP > lateP)
      earlyIndex -= incr;
    else break;
    incr /= 2.0;
    lateIndex = earlyIndex + 2.0;
  }

  maxIndex = earlyIndex + 1.0;
  maxVal = interpolatePoint(rxBurst,maxIndex);

  if (peakIndex!=NULL)
    *peakIndex = maxIndex;

  if (avgPwr!=NULL)
    *avgPwr = (sumPower-maxVal.norm2()) / (rxBurst.size()-1);

  return maxVal;

}

void scaleVector(signalVector &x,
		 complex scale)
{
  signalVector::iterator xP = x.begin();
  signalVector::iterator xPEnd = x.end();
  if (!x.isRealOnly()) {
    while (xP < xPEnd) {
      *xP = *xP * scale;
      xP++;
    }
  }
  else {
    while (xP < xPEnd) {
      *xP = xP->real() * scale;
      xP++;
    }
  }
}

/** in-place conjugation */
void conjugateVector(signalVector &x)
{
  if (x.isRealOnly()) return;
  signalVector::iterator xP = x.begin();
  signalVector::iterator xPEnd = x.end();
  while (xP < xPEnd) {
    *xP = xP->conj();
    xP++;
  }
}


// in-place addition!!
bool addVector(signalVector &x,
	       signalVector &y)
{
  signalVector::iterator xP = x.begin();
  signalVector::iterator yP = y.begin();
  signalVector::iterator xPEnd = x.end();
  signalVector::iterator yPEnd = y.end();
  while ((xP < xPEnd) && (yP < yPEnd)) {
    *xP = *xP + *yP;
    xP++; yP++;
  }
  return true;
}

// in-place multiplication!!
bool multVector(signalVector &x,
                 signalVector &y)
{
  signalVector::iterator xP = x.begin();
  signalVector::iterator yP = y.begin();
  signalVector::iterator xPEnd = x.end();
  signalVector::iterator yPEnd = y.end();
  while ((xP < xPEnd) && (yP < yPEnd)) {
    *xP = (*xP) * (*yP);
    xP++; yP++;
  }
  return true;
}


void offsetVector(signalVector &x,
		  complex offset)
{
  signalVector::iterator xP = x.begin();
  signalVector::iterator xPEnd = x.end();
  if (!x.isRealOnly()) {
    while (xP < xPEnd) {
      *xP += offset;
      xP++;
    }
  }
  else {
    while (xP < xPEnd) {
      *xP = xP->real() + offset;
      xP++;
    }      
  }
}

bool generateMidamble(signalVector &gsmPulse,
		      int samplesPerSymbol,
		      int TSC)
{

  if ((TSC < 0) || (TSC > 7)) 
    return false;

  if (gMidambles[TSC]) {
    if (gMidambles[TSC]->sequence!=NULL) delete gMidambles[TSC]->sequence;
    if (gMidambles[TSC]->sequenceReversedConjugated!=NULL)  delete gMidambles[TSC]->sequenceReversedConjugated;
  }

  signalVector emptyPulse(1); 
  *(emptyPulse.begin()) = 1.0;

  // only use middle 16 bits of each TSC
  signalVector *middleMidamble = modulateBurst(gTrainingSequence[TSC].segment(5,16),
					 emptyPulse,
					 0,
					 samplesPerSymbol);
  signalVector *midamble = modulateBurst(gTrainingSequence[TSC],
                                         gsmPulse,
                                         0,
                                         samplesPerSymbol);
  
  if (midamble == NULL) return false;
  if (middleMidamble == NULL) return false;

  // NOTE: Because ideal TSC 16-bit midamble is 66 symbols into burst,
  //       the ideal TSC has an + 180 degree phase shift,
  //       due to the pi/2 frequency shift, that 
  //       needs to be accounted for.
  //       26-midamble is 61 symbols into burst, has +90 degree phase shift.
  scaleVector(*middleMidamble,complex(-1.0,0.0));
  scaleVector(*midamble,complex(0.0,1.0));

  signalVector *autocorr = correlate(midamble,middleMidamble,NULL,NO_DELAY);
  
  if (autocorr == NULL) return false;

  gMidambles[TSC] = new CorrelationSequence;
  gMidambles[TSC]->sequence = middleMidamble;
  gMidambles[TSC]->sequenceReversedConjugated = reverseConjugate(middleMidamble);
  gMidambles[TSC]->gain = peakDetect(*autocorr,&gMidambles[TSC]->TOA,NULL);

  LOG(DEBUG) << "midamble autocorr: " << *autocorr;

  LOG(DEBUG) << "TOA: " << gMidambles[TSC]->TOA;

  //gMidambles[TSC]->TOA -= 5*samplesPerSymbol;

  delete autocorr;
  delete midamble;

  return true;
}

bool generateRACHSequence(signalVector &gsmPulse,
			  int samplesPerSymbol)
{
  
  if (gRACHSequence) {
    if (gRACHSequence->sequence!=NULL) delete gRACHSequence->sequence;
    if (gRACHSequence->sequenceReversedConjugated!=NULL) delete gRACHSequence->sequenceReversedConjugated;
  }

  signalVector *RACHSeq = modulateBurst(gRACHSynchSequence,
					gsmPulse,
					0,
					samplesPerSymbol);

  assert(RACHSeq);

  signalVector *autocorr = correlate(RACHSeq,RACHSeq,NULL,NO_DELAY);

  assert(autocorr);

  gRACHSequence = new CorrelationSequence;
  gRACHSequence->sequence = RACHSeq;
  gRACHSequence->sequenceReversedConjugated = reverseConjugate(RACHSeq);
  gRACHSequence->gain = peakDetect(*autocorr,&gRACHSequence->TOA,NULL);
 
  delete autocorr;

  return true;

}

				
bool detectRACHBurst(signalVector &rxBurst,
		     float detectThreshold,
		     int samplesPerSymbol,
		     complex *amplitude,
		     float* TOA)
{

  //static complex staticData[500];
 
  //signalVector correlatedRACH(staticData,0,rxBurst.size());
  signalVector correlatedRACH(rxBurst.size());
  correlate(&rxBurst,gRACHSequence->sequenceReversedConjugated,&correlatedRACH,NO_DELAY,true);

  float meanPower;
  complex peakAmpl = peakDetect(correlatedRACH,TOA,&meanPower);

  float valleyPower = 0.0; 

  // check for bogus results
  if ((*TOA < 0.0) || (*TOA > correlatedRACH.size())) {
        *amplitude = 0.0;
	return false;
  }
  complex *peakPtr = correlatedRACH.begin() + (int) rint(*TOA);

  LOG(DEBUG) << "RACH corr: " << correlatedRACH;

  float numSamples = 0.0;
  for (int i = 57*samplesPerSymbol; i <= 107*samplesPerSymbol;i++) {
    if (peakPtr+i >= correlatedRACH.end())
      break;
    valleyPower += (peakPtr+i)->norm2();
    numSamples++;
  }

  if (numSamples < 2) {
        *amplitude = 0.0;
        return false;
  }

  float RMS = sqrtf(valleyPower/(float) numSamples)+0.00001;
  float peakToMean = peakAmpl.abs()/RMS;

  LOG(DEBUG) << "RACH peakAmpl=" << peakAmpl << " RMS=" << RMS << " peakToMean=" << peakToMean;
  *amplitude = peakAmpl/(gRACHSequence->gain);

  *TOA = (*TOA) - gRACHSequence->TOA - 8*samplesPerSymbol;

  LOG(DEBUG) << "RACH thresh: " << peakToMean;

  return (peakToMean > detectThreshold);
}

bool energyDetect(signalVector &rxBurst,
		  unsigned windowLength,
		  float detectThreshold,
                  float *avgPwr)
{

  signalVector::const_iterator windowItr = rxBurst.begin(); //+rxBurst.size()/2 - 5*windowLength/2;
  float energy = 0.0;
  if (windowLength < 0) windowLength = 20;
  if (windowLength > rxBurst.size()) windowLength = rxBurst.size();
  for (unsigned i = 0; i < windowLength; i++) {
    energy += windowItr->norm2();
    windowItr+=4;
  }
  if (avgPwr) *avgPwr = energy/windowLength;
  LOG(DEBUG) << "detected energy: " << energy/windowLength;
  return (energy/windowLength > detectThreshold*detectThreshold);
}
  

bool analyzeTrafficBurst(signalVector &rxBurst,
			 unsigned TSC,
			 float detectThreshold,
			 int samplesPerSymbol,
			 complex *amplitude,
			 float *TOA,
			 unsigned maxTOA,
                         bool requestChannel,
                         signalVector **channelResponse,
			 float *channelResponseOffset) 
{

  assert(TSC<8);
  assert(amplitude);
  assert(TOA);
  assert(gMidambles[TSC]);

  if (maxTOA < 3*samplesPerSymbol) maxTOA = 3*samplesPerSymbol;
  unsigned spanTOA = maxTOA;
  if (spanTOA < 5*samplesPerSymbol) spanTOA = 5*samplesPerSymbol;

  unsigned startIx = 66*samplesPerSymbol-spanTOA;
  unsigned endIx = (66+16)*samplesPerSymbol+spanTOA;
  unsigned windowLen = endIx - startIx;
  unsigned corrLen = 2*maxTOA+1;

  unsigned expectedTOAPeak = (unsigned) round(gMidambles[TSC]->TOA + (gMidambles[TSC]->sequenceReversedConjugated->size()-1)/2);

  signalVector burstSegment(rxBurst.begin(),startIx,windowLen);

  //static complex staticData[200];
  //signalVector correlatedBurst(staticData,0,corrLen);
  signalVector correlatedBurst(corrLen);
  correlate(&burstSegment, gMidambles[TSC]->sequenceReversedConjugated,
					    &correlatedBurst, CUSTOM,true,
					    expectedTOAPeak-maxTOA,corrLen);

  float meanPower;
{'fraction_non_alphanumeric': 0.14829281563963545, 'fraction_numerical': 0.01657630346489438, 'mean_word_length': 2.9514704753129077, 'pattern_counts': {'":': 0, '<': 217, '<?xml version=': 0, '>': 261, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 10, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1854539', 'n_tokens_mistral': 20405, 'n_tokens_neox': 18913, 'n_words': 4329}