#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <limits.h>
#include <math.h>

#define __cplusplus__strings__
#ifndef M_PI
#define M_PI 3.14159265358979324
#endif

#include <sndfile.h>

//#if defined(WITH_SPANDSP_INTERNALS)
#define SPANDSP_EXPOSE_INTERNAL_STRUCTURES
//#endif

#include "minitel_fsk.h"
#include "spandsp.h"
#include "spandsp-sim.h"
int16_t tone_amp[160];
int tone_pos=0;

typedef struct
{
  double x, y;
} tComplex;

tComplex complexAdd(const tComplex* a, const tComplex* b)
{
  tComplex c;
  c.x = a->x + b->x;
  c.y = a->y + b->y;
  return c;
}

tComplex complexMul(const tComplex* a, const tComplex* b)
{
  tComplex c;
  c.x = a->x * b->x - a->y * b->y;
  c.y = a->x * b->y + a->y * b->x;
  return c;
}

void dft(tComplex out[], const tComplex in[], size_t n, int direction)
{
  size_t k, i;
  for (k = 0; k < n; k++)
  {
    tComplex r = { 0, 0 }, e;
    for (i = 0; i < n; i++)
    {
      e.x = cos(-2 * direction * M_PI / n * ((double)k - n / 2) * ((double)i - n / 2));
      e.y = sin(-2 * direction * M_PI / n * ((double)k - n / 2) * ((double)i - n / 2));
      e = complexMul(&e, &in[i]);
      r = complexAdd(&r, &e);
    }
    out[k] = r;
  }
}

#define FILTER_LENGTH 64
void TxInit(tTx* pTx,
            uint SampleRate,
            uint (*pTxGetDataCallBack)(tTx*, uint8*))
{
  memset(pTx, 0, sizeof(*pTx));
  pTx->State = stSendingOnes;
  pTx->SampleRate = SampleRate;
  pTx->OnesFreq = 1300;
  pTx->ZeroesFreq = 2100;
  pTx->BitRate = 1200;
  pTx->pTxGetDataCallBack = pTxGetDataCallBack;

  pTx->SampleCnt = 0;
  pTx->BitSampleCnt = pTx->SampleRate;
  pTx->Data = 0;
  pTx->DataLeft = 0;
  pTx->Phase = 0.0;
  pTx->PhaseIncrement = 2 * M_PI * pTx->OnesFreq / pTx->SampleRate;
  pTx->queue =  queue_init(NULL, 128, QUEUE_READ_ATOMIC | QUEUE_WRITE_ATOMIC);
}

int16 TxGetSample(tTx* pTx)
{
  int16 sample; 
  float sample1, sample2, mixed;

  if (pTx->State == stSendingOnes &&
      pTx->SampleCnt >= pTx->SampleRate)
  {
    // Sent 1 second worth of 1's, can now send data
    pTx->State = stSendingData;
  }

  if (pTx->State == stSendingData &&
      pTx->BitSampleCnt >= pTx->SampleRate)
  {
    // Another data bit can now be sent
    uint8 d;

    pTx->BitSampleCnt -= pTx->SampleRate;

    if (!pTx->DataLeft)
    {
      // Get the next data byte (if any)
      if (pTx->pTxGetDataCallBack(pTx, &d) != 0)
      {
        pTx->Data = d & 0x7F;
        pTx->Data |= (__builtin_popcount( d & 0x7F ) & 1) << 7; // insert parity
        pTx->Data |= 1 << 8; // insert the stop bit
        pTx->Data <<= 1; // insert the start bit
        pTx->DataLeft = 10;
      }
      else
      {
        pTx->Data = 0x3FF; // no data, send 10 1's
        pTx->DataLeft = 10;
      }
    }

    // Extract the next data bit to send
    d = pTx->Data & 1;
    pTx->Data >>= 1;
    pTx->DataLeft--;

    // Choose the appropriate frequency for 0 and 1
    if (d)
    {
      pTx->PhaseIncrement = 2 * M_PI * pTx->OnesFreq / pTx->SampleRate;
    }
    else
    {
      pTx->PhaseIncrement = 2 * M_PI * pTx->ZeroesFreq / pTx->SampleRate;
    }
  }

  // Generate the next sample, advance the generator's phase
  sample = (int16)(16000 * cos(pTx->Phase));
  pTx->Phase += pTx->PhaseIncrement;
  if (pTx->Phase >= 2 * M_PI)
  {
    pTx->Phase -= 2 * M_PI;
  }

  if (pTx->State == stSendingData)
  {
    pTx->BitSampleCnt += pTx->BitRate;
  }

  pTx->SampleCnt++;
  
  /*Mix tone*/
  sample1 = sample / 32768.0f;
  sample2 = tone_amp[tone_pos%sizeof(tone_amp)] / 32768.0f;
  mixed = sample1+sample2;
  mixed *= 0.75;
  if (mixed > 1.0f) { mixed = 1.0f ;printf("clip\n"); }
  if (mixed < -1.0f){mixed = -1.0f; printf("clip\n"); }
  
  tone_pos++;
  return (uint16_t)(mixed * 32768.0f);

}

void RxInit(tRx* pRx,
            uint SampleRate,
            void (*pRxGetDataCallBack)(struct tRx*, uint8))
{
  tComplex tmp[FILTER_LENGTH];
  uint i;

  memset(pRx, 0, sizeof(*pRx));
  pRx->State = stCarrierLost;
  pRx->SampleRate = SampleRate;
  pRx->OnesFreq = 1300;
  pRx->ZeroesFreq = 2100;
  pRx->MidFreq = (pRx->OnesFreq + pRx->ZeroesFreq) / 2;
  pRx->BitRate = 1200;
  pRx->pRxGetDataCallBack = pRxGetDataCallBack;

  pRx->SampleCnt = 0;
  pRx->BitSampleCnt = 0;
  pRx->Data = 0x3FF;
  pRx->Phase = 0.0;
  pRx->PhaseIncrement = 2 * M_PI * pRx->MidFreq / pRx->SampleRate;
  pRx->LastAngle = 0.0;
  pRx->LastDelta = 0;
  pRx->Deltas = 0;
  pRx->CarrierAngle = 0;
  pRx->CarrierCnt = 0;
  pRx->LongAvgPower = 0.0;
  pRx->ShortAvgPower = 0.0;
  pRx->queue = queue_init(rx->queue, 128, QUEUE_READ_ATOMIC | QUEUE_WRITE_ATOMIC);
  
  for (i = 0; i < FILTER_LENGTH; i++)
  {
    pRx->Delay[i] = 0.0;
  }

  for (i = 0; i < FILTER_LENGTH; i++)
  {
    if (i == 0) // w < 0 (min w)
    {
      pRx->Filter[i].x = 0;
      pRx->Filter[i].y = 0;
    }
    else if (i < FILTER_LENGTH / 2) // w < 0
    {
      pRx->Filter[i].x = 0;
      pRx->Filter[i].y = 0;
    }
    else if (i == FILTER_LENGTH / 2) // w = 0
    {
      pRx->Filter[i].x = 0;
      pRx->Filter[i].y = 0;
    }
    else if (i > FILTER_LENGTH / 2) // w > 0
    {
      pRx->Filter[i].x = 0;
      pRx->Filter[i].y = -1;

      // Extra filter to combat channel noise
      if (i - FILTER_LENGTH / 2 < 875UL * FILTER_LENGTH / pRx->SampleRate ||
          i - FILTER_LENGTH / 2 > (2350UL * FILTER_LENGTH + pRx->SampleRate - 1) / pRx->SampleRate)
      {
        pRx->Filter[i].y = 0;
      }
    }
  }

  memcpy(tmp, pRx->Filter, sizeof(tmp));
  dft(pRx->Filter, tmp, FILTER_LENGTH, -1);
}

#define RX_VERBOSE 0
void RxGetSample(tRx* pRx, int16 Sample)
{
  tComplex s = { 0.0, 0.0 }, ss;
  double angle;
  uint i;
  int delta;
  double pwr;

  // Insert the sample into the delay line
  memmove(&pRx->Delay[0], &pRx->Delay[1], sizeof(pRx->Delay) - sizeof(pRx->Delay[0]));
  pRx->Delay[FILTER_LENGTH - 1] = Sample;

  // Get the next analytic signal sample by applying Hilbert transform/filter
  for (i = 0; i < FILTER_LENGTH; i++)
  {
    s.x += pRx->Delay[i] * pRx->Filter[FILTER_LENGTH - 1 - i].x;
    s.y += pRx->Delay[i] * pRx->Filter[FILTER_LENGTH - 1 - i].y;
  }

  // Frequency shift by MidFreq down
  ss.x = cos(-pRx->Phase);
  ss.y = sin(-pRx->Phase);
  s = complexMul(&s, &ss);
  pRx->Phase += pRx->PhaseIncrement;
  if (pRx->Phase >= 2 * M_PI)
  {
    pRx->Phase -= 2 * M_PI;
  }

  // Calculate signal power
  pwr = (s.x * s.x + s.y * s.y) / 32768 / 32768;
  pRx->LongAvgPower *= 1 - pRx->BitRate / (pRx->SampleRate * 8.0 * 8);
  pRx->LongAvgPower += pwr;
  pRx->ShortAvgPower *= 1 - pRx->BitRate / (pRx->SampleRate * 8.0);
  pRx->ShortAvgPower += pwr;

#if 0
  printf("LongAvgPower:%f ShortAvgPower:%f\n", pRx->LongAvgPower, pRx->ShortAvgPower);
#endif

  // Disconnect if the signal power changes abruptly.
  if (pRx->State != stCarrierLost &&
      pRx->LongAvgPower > pRx->ShortAvgPower * 8 * 8)
  {
    // N.B. The receiver may have received a few extra (garbage) bytes
    // while demodulating the abruptly changed signal.
    // Prefixing data with its size or using a more advanced protocol
    // may be a good solution to this little problem.
    pRx->State = stCarrierLost;
    pRx->BitSampleCnt = 0;
    pRx->Data = 0x3FF;
    pRx->Phase = 0.0;
    pRx->LastAngle = 0.0;
    pRx->LastDelta = 0;
    pRx->Deltas = 0;
    pRx->CarrierAngle = 0;
    pRx->CarrierCnt = 0;
  }

  // Get the phase angle from the analytic signal sample
  angle = (fpclassify(s.x) == FP_ZERO && fpclassify(s.y) == FP_ZERO) ?
    0.0 : 180 / M_PI * atan2(s.y, s.x);
  // Calculate the phase angle change and force it to the -PI to +PI range
  delta = (int)(360.5 + angle - pRx->LastAngle) % 360;
  if (delta > 180) delta -= 360;

  if (pRx->State == stCarrierLost)
  {
    // Accumulate the phase angle change to see if we're receiving 1's
    pRx->CarrierAngle += delta;
    pRx->CarrierCnt++;

    // Check whether or not the phase corresponds to 1's
    if (delta < 0)
    {
      if (pRx->CarrierCnt >= pRx->SampleRate / pRx->OnesFreq * 8)
      {
        double ph = (double)pRx->CarrierAngle / pRx->CarrierCnt;
#if RX_VERBOSE
        printf("ca:%5ld, cc:%4ld, ca/cc:%4ld\n",
               (long)pRx->CarrierAngle,
               (long)pRx->CarrierCnt,
               (long)(pRx->CarrierAngle / pRx->CarrierCnt));
#endif
        // Frequency tolerance is +/-16 Hz per the V.23 spec
        if (ph < (pRx->OnesFreq - 17.0 - pRx->MidFreq) * 360.0 / pRx->SampleRate ||
            ph > (pRx->OnesFreq + 17.0 - pRx->MidFreq) * 360.0 / pRx->SampleRate)
        {
          goto BadCarrier;
        }
      }
    }
    else
    {
BadCarrier:
      // Phase doesn't correspond to 1's
      pRx->CarrierAngle = 0.0;
      pRx->CarrierCnt = 0;
    }

    if (pRx->CarrierCnt >= pRx->SampleRate / 2 + pRx->SampleRate / 4)
    {
      // 0.75 seconds worth of 1's have been detected, ready to receive data

      // Adjust MidFreq to compensate for the DAC and ADC sample rate difference
      double f1 = (double)pRx->CarrierAngle / pRx->CarrierCnt / 360 * pRx->SampleRate + pRx->MidFreq;
      pRx->MidFreq = (uint)(pRx->MidFreq * f1 / pRx->OnesFreq);
      pRx->PhaseIncrement = 2 * M_PI * pRx->MidFreq / pRx->SampleRate;
#if RX_VERBOSE
      printf("f1:%u, new MidFreq:%u\n", (uint)f1, pRx->MidFreq);
#endif
      pRx->State = stCarrierDetected;
    }
  }
  else
  {
    // Detect frequency changes (transitions between 0's and 1's)
    int freqChange = ((int32)pRx->LastDelta * delta < 0 || pRx->LastDelta && !delta);
    int reAddDelta = 0;

#if RX_VERBOSE
    printf("%6lu: delta:%4d freqChange:%d BitSampleCnt:%u\n",
           (ulong)pRx->SampleCnt,
           delta,
           freqChange,
           pRx->BitSampleCnt);
#endif

    // Synchronize with 1<->0 transitions
    if (freqChange)
    {
      if (pRx->BitSampleCnt >= pRx->SampleRate / 2)
      {
        pRx->BitSampleCnt = pRx->SampleRate;
        pRx->Deltas -= delta;
        reAddDelta = 1;
      }
      else
      {
        pRx->BitSampleCnt = 0;
        pRx->Deltas = 0;
      }
    }

    // Accumulate analytic signal phase angle changes
    // (positive for 0, negative for 1)
    pRx->Deltas += delta;

    if (pRx->BitSampleCnt >= pRx->SampleRate)
    {
      // Another data bit has accumulated
      pRx->BitSampleCnt -= pRx->SampleRate;

#if RX_VERBOSE
      printf("bit: %u\n", pRx->Deltas < 0);
#endif

      pRx->Data >>= 1;
      pRx->Data |= (pRx->Deltas < 0) << 9;
      pRx->Deltas = delta * reAddDelta;

      if ((pRx->Data & 0x201) == 0x200)
      {
        // Start and stop bits have been detected
        if (pRx->State == stCarrierDetected)
        {
          pRx->State = stReceivingData;
        }
        pRx->Data = (pRx->Data >> 1) & 0x7F;
        pRx->pRxGetDataCallBack(pRx, (uint8)pRx->Data);

#if RX_VERBOSE
        printf("byte: 0x%02X ('%c')\n",
               pRx->Data,
               (pRx->Data >= 0x20 && pRx->Data <= 0x7F) ? pRx->Data : '?');
#endif

        pRx->Data = 0x3FF;
      }
    }

    pRx->BitSampleCnt += pRx->BitRate;
  }

  pRx->LastAngle = angle;
  pRx->LastDelta = delta;
  pRx->SampleCnt++;
}

typedef struct
{
  tTx Tx;
  FILE* DataFile;
  int CountDown;
} tTxTest;

uint TxGetDataCallBack(tTx* pTx, uint8* pTxData)
{
  tTxTest* pTxTest = (tTxTest*)pTx;
  uchar c;

  if (pTxTest->CountDown)
  {
    pTxTest->CountDown--;
    return 0;
  }
  /*
  if (fread(&c, 1, 1, pTxTest->DataFile) != 1)
  {
    pTxTest->CountDown = 20;
    return 0;
  }
 */
 if( ! queue_empty(pTx->queue)){
    c =  queue_read_byte(pTx->queue);
    pTxTest->CountDown = 20;
   }
  *pTxData = c;
  return 1;
}

int testTx(uint SampleRate,
           double NoiseLevel,
           const char* DataFileName,
           const char* AudioFileName)
{
  FILE *fData = NULL;
  SNDFILE *fAudio = NULL;
  //FILE *fAudio = NULL;
  tone_gen_descriptor_t t_desc;
  tone_gen_state_t t_state;
    
  int err = EXIT_FAILURE;
  tTxTest txTest;

  if ((fData = fopen(DataFileName, "rb")) == NULL)
  {
    printf("Can't open file \"%s\"\n", DataFileName);
    goto Exit;
  }

  tone_gen_descriptor_init(&t_desc, 1300, -12.0, 0, 0,20, 0, 0, 0, 0);

  tone_gen_init(&t_state, &t_desc);
  tone_gen(&t_state, tone_amp, sizeof(tone_amp));

 if ((fAudio = sf_open_telephony_write(AudioFileName, 1)) == NULL)  
// if ((fAudio = fopen(AudioFileName, "wb")) == NULL)  
{
    printf("Can't create file \"%s\"\n", AudioFileName);
    goto Exit;
  }

  txTest.DataFile = fData;
  txTest.CountDown = 0;

  TxInit(&txTest.Tx,
         SampleRate,
         &TxGetDataCallBack);

  do
  {
    int16 sample = TxGetSample(&txTest.Tx);
    if (txTest.CountDown > 1 && txTest.CountDown <= 10)
    {
#if 0 // Enable this code to test disconnecting.
      // Finish with silence.
      
#endif
    }
    //sample += (rand() - (int)RAND_MAX / 2) * NoiseLevel * 16000 / (RAND_MAX / 2);
    sf_writef_short(fAudio, &sample, 1);
    //fwrite(&sample, 1, sizeof(sample), fAudio);
  } while (txTest.CountDown != 1); // Drain all data-containing samples

  err = EXIT_SUCCESS;

Exit:

  if (fData != NULL) fclose(fData);
  if (fAudio != NULL)
	sf_close(fAudio);
//	 fclose(fAudio);

  return err;
}

typedef struct
{
  tRx Rx;
  FILE* DataFile;
} tRxTest;

void RxGetDataCallBack(tRx* pRx, uint8 RxData)
{
  tRxTest* pRxTest = (tRxTest*)pRx;
  uchar c = RxData;
  if (queue_free_space(pRx->queue)){
     queue_write_byte(pRx->queue, (uint16_t)Rxdata); 
  }
  //fwrite(&c, 1, 1, pRxTest->DataFile);
}

int testRx(uint SampleRate,
           const char* AudioFileName,
           const char* DataFileName)
{
  uint lastState;
  int err = EXIT_FAILURE;
  tRxTest rxTest;


  RxInit(&rxTest.Rx,
         8000,//SampleRate,
         &RxGetDataCallBack);

  for (;;)
  {
    int16 sample;

    if (fread(&sample, 1, sizeof(sample), fAudio) != sizeof(sample))
    {
      if (rxTest.Rx.State != stCarrierLost) goto NoCarrier;
      break;
    }

    lastState = rxTest.Rx.State;
    RxGetSample(&rxTest.Rx, sample);

    if (rxTest.Rx.State != lastState && rxTest.Rx.State == stCarrierDetected)
    {
      printf("\nCONNECT %u\n\n", rxTest.Rx.BitRate);
    }

    if (rxTest.Rx.State != lastState && rxTest.Rx.State == stCarrierLost)
    {
NoCarrier:
      printf("\n\nNO CARRIER\n");
      break;
    }
  }

  err = EXIT_SUCCESS;

Exit:

  if (fAudio != NULL) fclose(fAudio);
  if (fData != NULL) fclose(fData);

  return err;
}
/*
int main(int argc, char* argv[])
{
  uint sampleRate;
  double noiseLevel;

  if (argc < 2 ||
      !strcasecmp(argv[1], "-help") ||
      !strcasecmp(argv[1], "/help") ||
      !strcasecmp(argv[1], "-?") ||
      !strcasecmp(argv[1], "/?"))
  {
Usage:
    printf("Usage:\n\n"
           "  %s tx <sample rate> <noise level> <data input file> <PCM output file>\n"
           "  %s rx <sample rate> <PCM input file> <data output file>\n",
           argv[0],
           argv[0]);
    return 0;
  }

  if (!strcasecmp(argv[1], "tx") &&
      argc == 6 &&
      sscanf(argv[2], "%u", &sampleRate) == 1 &&
      sscanf(argv[3], "%lf", &noiseLevel) == 1)
  {
    return testTx(sampleRate, noiseLevel, argv[4], argv[5]);
  }
  else if (!strcasecmp(argv[1], "rx") &&
           argc == 5 &&
           sscanf(argv[2], "%u", &sampleRate) == 1)
  {
    return testRx(sampleRate, argv[3], argv[4]);
  }
  else
  {
    goto Usage;
  }
}
*/