/***************************************************************************
                          mfskdemodulator.cpp  -  description
                             -------------------
    begin                : Mit Jan 29 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 "mfskdemodulator.h"
//#include "deinterleaver.h"
#include "viterbi.h"
#include "constants.h"
#include "mfskvaricode.h"


const double MFSKDistance(double xa,int b)
{
unsigned int count,c;
int a;
a= (int) xa;
c = a ^ b;
count =0;
while( c > 0 )
  {
   if (c & 1) count++;
    c = c >>1;
  }
return double(count);

}

MFSKDemodulator::MFSKDemodulator():FSKDemodulator(16)
{
Decoder=new MFSKVaricode();
viterbi = new Viterbi(7,0x6D,0x4F,MFSKDistance);
//leave = new Deinterleaver[10]();
Bitshiftregister=0;
}
MFSKDemodulator::~MFSKDemodulator()
{
  
//if ( leave != 0)
//  delete leave;
if ( viterbi)
  delete viterbi;
if ( Decoder !=0)
  delete Decoder;
}

void MFSKDemodulator::ProcessInput(double * input, double *)
{
int SamplestoProcess;
int i,j;
int SamplesperBit=16;
unsigned char Symbols[4];
int Symbol;
//Zum Testen
//double energy[94][16];
//double phase[94][16];
double energy[94*16];
double phase[94*16];
mixer(input);
SamplestoProcess=lpDownSample();

for(i=0;i<SamplestoProcess;i++)
 {
  for(j=0;j<NumberofTones;j++)
   {
//    energy[i][j]=abs(FilterOutput[j][i]);
//    phase[i][j]=arg(FilterOutput[j][i]);
    energy[i*16+j]=abs(FilterOutput[j][i]);
    phase[i*16+j]=arg(FilterOutput[j][i]);
   }
 }  
i=0;
while ( i < SamplestoProcess)
 {
  decodesymbol(&energy[i*16],Symbols);
  for(j=0;j < 10; j++)
   leave[i].deinterleave(Symbols);
/**
leave0.deinterleave(Symbols);
leave1.deinterleave(Symbols);
leave2.deinterleave(Symbols);
leave3.deinterleave(Symbols);
leave4.deinterleave(Symbols);
leave5.deinterleave(Symbols);
leave6.deinterleave(Symbols);
leave7.deinterleave(Symbols);
leave8.deinterleave(Symbols);
leave9.deinterleave(Symbols);
**/
  // Take two bits from Symbols and feed into viterbi decoder 
  j=0;
  if ( Symbols[0] == '1')
   j=2;
  if (Symbols[1] == '1')
   j = j+1;
  viterbi->decode(j);
  recvbit(viterbi->getbit(48));
  // Take next two bits from Symbols and feed into viterbi decoder 
  j=0;
  if ( Symbols[2] == '1')
   j=2;
  if (Symbols[3] == '1')
   j = j+1;
  viterbi->decode(j);
  recvbit(viterbi->getbit(48));   
  i +=SamplesperBit;
 }  
}

int MFSKDemodulator::getSquelchValue()
{

return 0;
}
bool MFSKDemodulator::Init(double FS,int NumberofSamples)
{
Baudrate=15.625;
SymbolLength=(int) (FS/Baudrate+0.5);
FSKDemodulator::Init(FS,NumberofSamples);
setFilter(Baudrate/2,705,44);
setRxFrequency(1000.);
}


void MFSKDemodulator::decodesymbol(double *in,unsigned char * s)
{
double x, max = 0.0;
int i, symbol=0;
for (i = 0; i < NumberofTones; i++)
 {
  if ((x = in[i]) > max)
   {
    max = x;
    symbol = i;
   }
 }

// Graydecode Number and convert to char stream 
// Use char stream to be able to use soft decode later
for(i=0; i <4; i++)
 s[i]=GraydecodeTable[symbol][i];
}
void MFSKDemodulator::recvbit(int bit)
{
	int c;
  char zeichen;
	Bitshiftregister = (Bitshiftregister << 1);
  c = !! bit;
  Bitshiftregister = Bitshiftregister | c;

	/* search for "001" */
  c = Bitshiftregister & 7;
	if ((Bitshiftregister & 7) == 1)
   {
		/* the "1" belongs to the next symbol */
		c = Decoder->decode(Bitshiftregister >> 1);

  zeichen = c;
  if ( c != -1 )
		emit newSymbol((char) c);
		/* we already received this */
		Bitshiftregister = 1;
	}
}

void MFSKDemodulator::synchronize()
{
}

void MFSKDemodulator::afc()
{
}

double MFSKDemodulator::get2RxFrequency()
{
return RxFrequency+ NumberofTones*Baudrate;
}
void MFSKDemodulator::setRxFrequency(double freq)
{
 if ( freq != RxFrequency)
 {
  RxFrequency=freq;
  for(int i=0;i <NumberofTones;i++) 
  mixerfreqinc[i]=PI2*(RxFrequency+i*Baudrate)/SampleRate;
 }
}
AfcMode MFSKDemodulator::AfcProperties()
{
return Narrow;
}