/***************************************************************************
                          fskdemodulator.cpp  -  description
                             -------------------
    begin                : Fre Jan 31 2003
    copyright            : (C) 2003 by Volker Schroer
    email                : dl1ksv@gmx.de
 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *   The PSK part is based on WinPSK 1.0 by Moe Wheatly, AE4JY             *
 ***************************************************************************/

#include "fskdemodulator.h"

FSKDemodulator::FSKDemodulator(int NxFrequencies):CDemodulator()
{
NumberofTones=NxFrequencies;
mixerfreq = new double[NumberofTones];
for (int i=0; i < NumberofTones; i++ )
 mixerfreq[i]=0.;
mixerfreqinc= new double[NumberofTones];
//*mixedfreqs= new complex<double>[NumberofTones];
//*FilterBuffer= new complex<double>[NumberofTones];
//*FilterOutput= new complex<double>[NumberofTones];
RestSamples=0;
}
FSKDemodulator::~FSKDemodulator()
{
if ( mixerfreq )
 delete mixerfreq;

if ( mixerfreqinc)
  delete mixerfreqinc;

if (*mixedfreqs != 0)
 {
  for(int i=0; i< NumberofTones;i++)
   if (mixedfreqs[i] != 0)
    delete mixedfreqs[i];
 }     
if (*FilterOutput != 0)
 {
  for(int i=0; i< NumberofTones;i++)
   if (FilterOutput[i] != 0)
    delete FilterOutput[i];
 }
if (*FilterBuffer != 0)
 {
  for(int i=0; i< NumberofTones;i++)
   if (FilterBuffer[i] != 0)
    delete FilterBuffer[i];
 } 
if( lpcoefs !=0)
  delete lpcoefs;
}

bool FSKDemodulator::Init(double fs,int Samples)
{
NxSamples=Samples;
SampleRate=fs;

for(int i=0; i< NumberofTones;i++)
  mixedfreqs[i]=new complex<double>[NxSamples];
return true;  
}
void FSKDemodulator::mixer(double *input)
{
double z;
for (int i=0; i < NxSamples; i++)
 {
  z=input[i];
  for(int j=0; j<NumberofTones;j++)
   {
    ;
    mixedfreqs[j][i]=z*complex<double>(cos(mixerfreq[j]),sin(mixerfreq[j]));  
    mixerfreq[j] +=mixerfreqinc[j];
    if( mixerfreq[j] > PI2 )
     mixerfreq[j] -=PI2;
   }
 }
}
int FSKDemodulator::lpDownSample()
{
int k;
complex<double> *FilterBufferPtr,*FilterBufferLoopPtr,*OutputPtr,acc;
complex<double> *FilterInputPtr;
double *FilterPtr;

for (int j=0;j < NumberofTones;j++)
 {
  FilterBufferPtr=FilterBuffer[j];
  k=RestSamples;
  FilterBufferLoopPtr=SaveFilterBufferPtr[j];;
  OutputPtr=FilterOutput[j];
  FilterInputPtr=mixedfreqs[j];
  for (int i=0;i < NxSamples;i++)
   {
    *FilterBufferLoopPtr=FilterInputPtr[i];
    if ( (k % DownRate ) == 0)
     { // Filter
      FilterPtr=lpcoefs;
      acc=complex<double>(0.,0.);
      for(int l=0;l <  FilterLength; l++)
       {
        acc +=(*FilterPtr++)*(*FilterBufferLoopPtr--); // x(n-j)*h(j)
        if (FilterBufferLoopPtr < FilterBufferPtr )
         FilterBufferLoopPtr = FilterBufferPtr+FilterLength-1;  
        }	
       if(++FilterBufferLoopPtr >= (FilterBufferPtr+FilterLength) )
        FilterBufferLoopPtr=FilterBufferPtr;
       *OutputPtr++=acc;     
     } //End Filter
     else
      if(++FilterBufferLoopPtr >= (FilterBufferPtr+FilterLength) )
       FilterBufferLoopPtr=FilterBufferPtr;     
    k++; 
   }
//Save Filter Pointer  
  SaveFilterBufferPtr[j]=FilterBufferLoopPtr; 
 }
RestSamples = (RestSamples+NxSamples)%DownRate;
k=OutputPtr-FilterOutput[NumberofTones-1];
return k; 
}
void FSKDemodulator::setFilter(double Frequency ,int NxFilter,int Rate)
{
double x0;
int i,j;
x0=PI2*Frequency/SampleRate;
FilterLength=NxFilter;
DownRate=Rate; 
lpcoefs= new double[FilterLength];


j = (FilterLength-1)/2;
for(i=0;i < FilterLength; i++)
 {
  if( i != j )
   lpcoefs[i]=sin(x0*(i-j))/(i-j);
  else
   lpcoefs[i]=x0;
  lpcoefs[i] *=(0.42-0.5*cos((PI2*i)/(FilterLength-1))
   +0.08*cos((PI2*(i+i))/(FilterLength-1)));
//   lpcoefs[i]=1.;
 }
  // Normalize for unity at DC
x0=0.;
for(i=0;i<FilterLength;i++) 
 x0 +=lpcoefs[i];
x0 /=10.;
for(i=0;i<FilterLength;i++) 
 lpcoefs[i] /=x0;
 
for(j=0;j < NumberofTones;j++)
 {
  FilterBuffer[j] =new complex<double>[FilterLength];
  SaveFilterBufferPtr[j]=FilterBuffer[j];  
  for(i=0;i<FilterLength;i++)
   FilterBuffer[j][i]=complex<double>(0.,0.);   
 }

 
j=NxSamples/DownRate+1;
for(i=0;i< NumberofTones;i++)
 FilterOutput[i]=new complex<double>[j];

}