/***************************************************************************
                          rttydemodulator.cpp  -  description
                             -------------------
    begin                : Mon Jun 4 2001
    copyright            : (C) 2001 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.                                   *
 ***************************************************************************/

#include "rttydemodulator.h"
#include "firfilter.h"
#include "constants.h"

//#define CC1 0.9491 //   0.875 0.125 0.9125 0.0875 0.2 0.8
//#define CC3 0.05518// 0.9 0.02
//#define B1 0.932
#define CC1 0.95    /// 1. 0.1 0.7 , 0.775 0.1 0.7 , 0.8 0.08 0.575
#define CC3 0.15     /// 0.975 0.05 0.55 , 0.95 0.06 0.4 , 0.975 0.2 0.625
#define B1  0.625
#define DOWN 2
#define HoldLimit 3
RTTYDemodulator::RTTYDemodulator():FSKDemodulator(2)
{
ShiftOn = false;
BufferPointer=0;
BufferCount=0;
StopBit1Length=0;
RxFrequency=0.0; // Will be set correctly in setFrequency !
//Initialize ExtraParameter
extraParameter.stopbits=Onepoint5;
extraParameter.parity=None;
extraParameter.reverse=true;
extraParameter.offset=170;

C1=CC1;
C3=CC3;
}
RTTYDemodulator::~RTTYDemodulator()
{
 /**
 if (Lp0 != 0)
  delete Lp0;
 if (Lp1 != 0)
  delete Lp1;
 if (Lp2 != 0)
  delete Lp2;

 Lp0=0;
 Lp1=0;
 Lp2=0;
 **/
}
/** returns the asci char corresponding to the baudot code */
char RTTYDemodulator::baudot_code(char data)
{
/** Table of letters */

static const char letters[32] =	{0x00,'E','\r','A',' ','S','I','U',
				 '\n','D','R','J','N','F','C','K',
				 'T','Z','L','W','H','Y','P','Q',
				 'O','B','G','^','M','X','V','^'};


/** Table of symbols */
static const char symbols[32] = {0x00,'3','\r','-',' ','\'','8','7',
                                  '\n','$','4','#',',','!',':','(',
                                   '5','"',')','2','#','6','0','1',
                                   '9','?','&','^','.','/',';','^'};

char c;

 switch (data)
 {
  case 0x1f :
   ShiftOn = false;  //LTRS
   c = 0;
   break;
  case 0x1b :
   ShiftOn = true;   //FIGS
   c = 0;
   break;
  default:
   if (!ShiftOn)
    c=letters[(int) data];
   else
    c=symbols[(int) data];
    break;
 }


if ( c == ' ')	// Unshift on Space
 ShiftOn =false;	

return c;
}

bool RTTYDemodulator::Init(double FS,int NumberofSamples)
{
 double coeffs[NumberofProbes], x0;
 int i;
FSKDemodulator::Init(FS,NumberofSamples);
Baudrate=45.45;
NumberOfBits=5;
SymbolLength=int (FS/Baudrate+0.5);
Status = WaitingForMark;
FrequencyChanged = false;
ave1=0.5;
ave2=0.0;
setFilter(Baudrate,SymbolLength,Distance);
setRxFrequency(1000.);
DisplayPointer=0;

x0 = PI2*85./(11025./Distance);
for(i=0; i < NumberofProbes; i++)
 {
  if( i != (NumberofProbes-1)/2 )
   coeffs[i]=sin(x0*(i-(NumberofProbes-1)/2))/(i-(NumberofProbes-1)/2);
  else
   coeffs[i]=x0;
  coeffs[i] *=(0.42-0.5*cos((PI2*i)/(NumberofProbes-1))
                +0.08*cos((PI2*(i+i))/(NumberofProbes-1)));
 }
  // Normalize for unity at DC
  x0=0.;
  for(i=0;i<NumberofProbes;i++) 
   x0 +=coeffs[i];
  for(i=0;i<NumberofProbes;i++) 
   coeffs[i] /=x0;
DiscriminatorThreshold=0.;
MarkMax=0.05;
SpaceMax=0.05;
holdCount1=0;
holdCount2=0;
return true;
}

void RTTYDemodulator::ProcessInput(double *input, double *)
{
char c1;
int i,j,count;
int StartBitCount,StopBit2Length, StartBitLength;
int StopBit2Count;
int actSample;
int SamplestoProcess;
actSample = 0;
double F0in[MaxSamplestoProcess], F1in[MaxSamplestoProcess];
 //, F0out[MaxSamplestoProcess], F1out[MaxSamplestoProcess];
//double ThresholdInput[MaxSamplestoProcess],ThresholdOutput[MaxSamplestoProcess];

float xx,xSum,StartBitValue, StopBit2Value;

mixer(input);
SamplestoProcess=lpDownSample();
for(i=0;i <SamplestoProcess;i++)
{
 F0in[i]=abs(FilterOutput[0][i]);
 F1in[i]=abs(FilterOutput[1][i]); 
}


while ( actSample < SamplestoProcess )
{
 if( (BufferCount < SampleBufferLength ) )
 { 
  while ( (BufferCount < SampleBufferLength ) && ( actSample < SamplestoProcess ) )
  {
    BufferCount++;

    ellipseDisplay[DisplayPointer]=complex<float>(0.,5.);
    DisplayPointer = (DisplayPointer +1 )% 512;
    if (extraParameter.reverse)
    {
     MarkBuffer[BufferPointer]=F1in[actSample];
     SpaceBuffer[BufferPointer]=F0in[actSample];
    } 
    else
    { 
     MarkBuffer[BufferPointer]=F0in[actSample];
     SpaceBuffer[BufferPointer]=F1in[actSample];
    } 
    if(MarkMax < MarkBuffer[BufferPointer] )
    {
     MarkMax = MarkMax + C1 * ( MarkBuffer[BufferPointer]-MarkMax);
     holdCount1=0;
    }
    else
    {
     if ( holdCount1 == HoldLimit )
      MarkMax = MarkMax - C3 * ( MarkMax -MarkBuffer[BufferPointer]);
     else
      holdCount1++;
    }
    if(SpaceMax < SpaceBuffer[BufferPointer] )
    {
     SpaceMax = SpaceMax + C1 * ( SpaceBuffer[BufferPointer]-SpaceMax);
     holdCount2=0;
    }
    else
    {
     if ( holdCount2 == HoldLimit )
      SpaceMax = SpaceMax - C3 * ( SpaceMax -SpaceBuffer[BufferPointer]);
     else
      holdCount2++; 
    }
    MarkBuffer[BufferPointer] -= MarkMax/2;
    SpaceBuffer[BufferPointer] -= SpaceMax/2;
    
    CharacterData[BufferPointer]=MarkBuffer[BufferPointer]-SpaceBuffer[BufferPointer];
    BufferPointer++;
    BufferPointer=BufferPointer%SampleBufferLength;
    actSample ++;
  } // End of filling CharacterData
 }
 while (BufferCount == SampleBufferLength)
 {  
  switch (Status)  // Now let's analyze the data
  {
   case WaitingForMark:        // Waiting for Stopbit, previous state undefined 

    switch(extraParameter.stopbits)
    {
     case One:
      StopBit1Length=NumberofProbes;
      break;
     case Onepoint5:
      StopBit1Length =(3*NumberofProbes)/2;
      break;
      
     case Two:
      StopBit1Length=2*NumberofProbes;
      break;  
    }
    // Check, if we are possibly at the beginning of a stop bit
    
    i = 0;
    while ( (i < BufferCount ) && (CharacterData[(BufferPointer + i) % SampleBufferLength] <= DiscriminatorThreshold ) )
     i++;
    if ( i == 0 ) // At the beginning
    {
     StopBit1Count=0;
     xSum=0.;
     StopBit1Value=0.;
     while ( i < StopBit1Length )
     {
      if (CharacterData[(BufferPointer + i) % SampleBufferLength] > DiscriminatorThreshold )
       StopBit1Count++;
      StopBit1Value +=MarkBuffer[(BufferPointer + i) % SampleBufferLength];
      xSum += CharacterData[(BufferPointer + i) % SampleBufferLength];
      i++;
     }
     StopBit1Value= StopBit1Value/StopBit1Length;
     xSum = xSum / StopBit1Length;
     if ( xSum > DiscriminatorThreshold  ) 
      Status = WaitingForSpace;
     else
      BufferCount -= StopBit1Count;
    }
    else         // Refill the buffer
     BufferCount -=i;
   break;
    
    case WaitingForSpace:      // Stopbit seems to be found, now waiting for transition
    
     i = StopBit1Length  ;
     while( (i < SampleBufferLength) && (CharacterData[(BufferPointer + i) % SampleBufferLength ]
                                         > DiscriminatorThreshold ) )
      i++;
     if (i == SampleBufferLength )
     {
      BufferCount= StopBit1Length;   // No Space found, keep only StopBit1Length Samples
      DiscriminatorThreshold /=DOWN;
      CalcQuality((float)0.);
     }
     else
     {
      Status=CheckingStartBit;
      i = i - StopBit1Length; 
      BufferCount -=i;              // Refill buffer
     }
     break;
     
    case CheckingStartBit:
     
     j = StopBit1Length + BufferPointer;
     xSum =0.;
     StartBitValue = 0.;
     StartBitCount=0;
     StartBitLength = NumberofProbes;
     for ( i= 0; i < NumberofProbes; i++)
     { 
      if (CharacterData[( i + j ) % SampleBufferLength]  < DiscriminatorThreshold )
       StartBitCount++;
      StartBitValue += SpaceBuffer[( i + j ) % SampleBufferLength];
      xSum += CharacterData[( i + j ) % SampleBufferLength];
     }
     
     StartBitValue= StartBitValue / StartBitLength; 
     xSum = xSum/StartBitLength;
     if ( xSum < DiscriminatorThreshold ) // Super, Startbit fits very well
     {
      Status = CheckingStopBits;
      DiscriminatorThreshold = B1 * DiscriminatorThreshold + (1.-B1) * (StopBit1Value - StartBitValue ) /2; 
     if(abs(xSum) > 0.01)
      CalcQuality(abs(StartBitValue/xSum));
     else
      CalcQuality(abs(xSum));
     }
     else
     {
       Status = WaitingForMark;           // Was'nt the correct start bit, neither number nor value fits
       BufferCount -= StopBit1Length;
       CalcQuality((float)0.);
     } 
     
     break;
     
     case CollectingByte:
      c1=0;
      for(i=0;i < NumberOfBits; i++)  
      {
       j=(BufferPointer + StopBit1Length + StartBitLength + i * NumberofProbes ) % SampleBufferLength;
       xx =0.;
       count = 0;
       for(int j1=j;j1 <j + NumberofProbes; j1++)
        xx += CharacterData[j1];
       xx /= NumberofProbes;
       
       if ( xx > DiscriminatorThreshold )
       {
        c1 |= ( 1 << i);
        DiscriminatorThreshold =B1 * DiscriminatorThreshold + (1.-B1) * (xx - StopBit1Value)/2 ;
        StopBit1Value=xx;
       }
       else 
       {
        DiscriminatorThreshold =B1 * DiscriminatorThreshold + (1.-B1) * (StartBitValue-xx)/2 ;
        StartBitValue=xx;
       }
      }  
      if ( (c1 > 0 ) && (!Squelch || (Squelch && ( (unsigned int)(100.*ave1)>CDemodulator::Threshold))))
      {
       c1 = baudot_code(c1);
       if ( c1 > 0)           // FIGS or LTRS result in c1 = 0 !
        emit newSymbol( c1 );
      } 
     
      if (extraParameter.parity != None)
       Status = CheckingParity;
      else
      {
       if ( xSum > DiscriminatorThreshold ) //|| (StopBit2Value > DiscriminatorThreshold) )
        Status = WaitingForSpace;
       else
        Status=WaitingForMark;
       BufferCount -= (StopBit1Length + StartBitLength + 5*NumberofProbes  );
       StopBit1Length=StopBit2Length;
       StopBit1Count=StopBit2Count;
       StopBit1Value=StopBit2Value;
      }
     break;
      
     case CheckingParity: // Here we need BitsInData
     break;
      
     case CheckingStopBits:
      switch(extraParameter.stopbits)
      {
       case One:
        StopBit2Length=NumberofProbes;
        break;
       case Onepoint5:
        StopBit2Length =(3*NumberofProbes)/2;
        break;
        
       case Two:
        StopBit2Length=2*NumberofProbes;
        break;  
      }
      
      xSum=0.;
      StopBit2Count=0;
      StopBit2Value=0.;
      j = BufferPointer+StopBit1Length + StartBitLength + NumberOfBits*NumberofProbes;
      for ( i=0; i < StopBit2Length;i++)
      { 
       xx = CharacterData[(j+i)%SampleBufferLength];
       StopBit2Value += MarkBuffer[(j+i)%SampleBufferLength];
       if ( xx > DiscriminatorThreshold)
        StopBit2Count++;
       xSum +=xx;
      }
      StopBit2Value = StopBit2Value/StopBit2Length; 
      xSum /= StopBit2Length;
      if ( xSum > DiscriminatorThreshold ) //|| ( StopBit2avgThreshold > DiscriminatorThreshold ) )
       Status = CollectingByte; 
      else
      {
       if (StartBitValue < DiscriminatorThreshold ) 
       {
        
       Status = CollectingByte; 
       }
       else
       {
       // StartBit was not Ok, can we correct ?
        Status = WaitingForMark;
        BufferCount -= (StopBit1Length+ StartBitLength/2);
       }
      }
     break; 
  }  // end of switch
 }
 }
}
void RTTYDemodulator::setRxFrequency(double freq)
{
 if ( freq != RxFrequency)
 {
  RxFrequency=freq; 
  mixerfreqinc[0]=PI2*(RxFrequency)/SampleRate;
  mixerfreqinc[1]=PI2*(RxFrequency+extraParameter.offset)/SampleRate;
 }
}
void RTTYDemodulator::CalcQuality(float x)
{
 ave2=ave1;
 ave1=0.3*ave1 + 0.15 *ave2 + 0.55 * x;
// ave1 = 0.4899*ave1 + 0.9165 *ave2 + 0.916572 *x;
}
void RTTYDemodulator::CalcQuality(int pointer)
{
ave2=ave1;
float sum,diff;
pointer = pointer % SampleBufferLength;
diff=abs(CharacterData[pointer]);
sum=MarkBuffer[pointer]+SpaceBuffer[pointer];

//if ( sum > 0.1 )
 ave1=0.7*ave1 + 0.25 * ave2 + 0.05 * diff/sum;
//else
// ave1 = 0.5*ave1 + 0.3 *ave2;
}

int RTTYDemodulator::getSquelchValue()
{
return (int)(100.*ave1);
}

double RTTYDemodulator::get2RxFrequency()
{
 return RxFrequency+extraParameter.offset;

}
void RTTYDemodulator::setParameter(RxTxParameterType Type,void *Value)
{
switch (Type)
 {
 case Reverse:
        extraParameter.reverse =  * (bool *) Value;
        break;
 case Offset:
        extraParameter.offset = * (int *) Value;
        break;
case Parity:
        extraParameter.parity = * (Paritaet *) Value;
        break;
case Extra:
       extraParameter = * (ExtraParameter *) Value;
       break; 
 default:
        break;
 }
}
void *RTTYDemodulator::getParameter(RxTxParameterType Type)
{
switch (Type)
 {
 case Reverse:
        return (void *) &extraParameter.reverse; 
        break;
 case Offset:
        return (void *) &extraParameter.offset;
        break;
case Parity:
        return (void *) &extraParameter.parity;
        break;
case Extra:
        return (void *) &extraParameter;
        break;        
 default:
        return 0;
        break;
  }      
}
void *RTTYDemodulator::getBuffer()
{
return (void *) ellipseDisplay;
}
AfcMode RTTYDemodulator::AfcProperties()
{
return Off;
}