/*
    ACfax - Fax reception with X11-interface for amateur radio
    Copyright (C) 1995-1998 Andreas Czechanowski, DL4SDC

    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.

    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 General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software Foundation,
    Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.

    andreas.czechanowski@ins.uni-stuttgart.de
*/
    
/*
 * mod_demod.c - FM and AM modulator/demodulator for ACfax
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <unistd.h>
#include "mod_demod.h"

SHORT int firwide[] = {  6,   20,   7, -42, -74, -12, 159, 353, 440 };
SHORT int firmiddle[] = {   0, -18, -38, -39,   0,  83, 191, 284, 320 };
SHORT int firnarrow[] = {  -7, -18, -15,  11,  56, 116, 177, 223, 240 };

/* these some more coefficient tables were for testing only. */
/* int firwide[] = {  12,  24,   7, -42, -74, -12, 159, 353, 440 }; */
/* int firnarrow[] = { -16, -21, -15,  11,  56, 116, 177, 223, 240 }; */
/* int fircheap[] = { 0, 0, 0, 0, 0, 0, 36, 99, 128 }; */
/* int firtoobig[] = { -14, -12, 18, 40, -1, -80, -71, 114, 381, 510 } */
/* int firnotbad[] = { 11, 16, 1, -29, -44, -3, 98, 208, 256 }; */

static SHORT int *fcoeff = firmiddle;
static int *sqrt_lo;	/* square-root lookup-table for AM demodulator */
static int *sqrt_hi; 	/* (splitted in 2 parts) */
static int *sintab;	/* sine wave table for FM modulator */
static int *asntab;	/* arcsin-table for FM-demodulator */
static int *fmphsinc;	/* phase-increment-table for FM modulator */
static char *amreal;	/* amplitude tables for AM modulator */
static int compval;	/* comparator value for APT detecion */

/*
void main(int argc, char **argv)
{
  char indata[2048];
  int  outdata[1024];
  int  q;

  modem_init();
  set_modem_param (FIL_MIDL, 255, 400);
  while ((q = fread(indata, 1, 2048, stdin)) > 0) {
    q &= 0xfffe;
    fm_demod(indata, q, outdata);
  }
}
*/

/* Initialize the modulator/demodulator functions. This function allocates
   space for the arrays and lookup-tables
*/

void modem_init(void)
{
  int i;
  static int inited = 0;

  if (inited) return;

  sqrt_lo = (int *)malloc(2048*sizeof(int));
  sqrt_hi = (int *)malloc(2048*sizeof(int));
  if (!(sqrt_lo) || !(sqrt_hi)) {
    perror("modem_init:sqrt_*");
    exit(1);
  }
  amreal = (char *)malloc(258);
  if (!(amreal)) {
    perror("modem_init:am{real|imag}");
    exit(1);
  }
  sintab = (int *)malloc(1024*sizeof(int));
  if (!(sintab)) {
    perror("modem_init:sintab");
    exit(1);
  }
  asntab = (int *)malloc(512*sizeof(int));
  if (!(asntab)) {
    perror("modem_init:asntab");
    exit(1);
  }
  fmphsinc = (int *)malloc(258*sizeof(int));
  for (i=0; i<1024; i++)
    sintab[i] = 127.5 + 127*sin(i*M_PI/512.0); 

  inited = -1;
}

/* set up a lookup-table for the given deviation which gives values from
   0 to maxval with clipping beyond the edges for reception, initialize
   a lookup-table for FM transmission.
*/
void set_modem_param(int filter, int maxval, int devi)
{
  int phmax;
  int i;

  /* initialize decoder first, if not already done */
  modem_init();
  /* do some range-checking */
  if (devi < 100) devi = 100;
  if (devi > 1200) devi = 1200;
  if (maxval < 1) maxval = 1;
  if (maxval > 255) maxval = 255;
  phmax = 255.9 * sin(devi * M_PI / 4000.0) + 0.5;
  for (i=0; i<512; i++) {
    if (i <= 256-phmax)
      asntab[i] = 0;
    else if (i >= 256+phmax)
      asntab[i] = maxval;
    else
      asntab[i] = maxval * 2000 / devi * asin((i-256.0)/256.5) / M_PI
		+ maxval/ 2;
  }
  for (i=0; i<2048; i++) {
    sqrt_lo[i] = (sqrt(i) * maxval / 256.0) + 0.5;
    sqrt_hi[i] = (sqrt(32*i) * maxval / 256.0) + 0.5;
  }
  for (i=0; i<=maxval; i++)
    fmphsinc[i] = devi / 2000.0 * 65536;
  for (i=0; i<= maxval; i++)
    amreal[i] = 127.5 + 127.5*i/maxval;
  compval = maxval >> 1;
  switch (filter) {
    case FIL_NARR:
      fcoeff = firnarrow;
      fprintf(stderr, "selecting narrow filter\n");
      break;
    case FIL_MIDL:
      fcoeff = firmiddle;
      fprintf(stderr, "selecting middle filter\n");
      break;
    case FIL_WIDE:
      fcoeff = firwide;
      fprintf(stderr, "selecting wide filter\n");
      break;
  }
}

/* Demodulate the samples taken with 8kHz to a frequency sampled with 4kHz.
   This is done with a delay-demodulator :

               +----------+  +-------+     +----+  +---+
            +->|*sin(2kHz)|->|FIR-LPF|--*->|z^-1|->|mul|--+
            |  +----------+  +-------+  |  +----+  +---+  |
            |                           *-->--     /      |     AM^2
            |                           |     \   /       |+     |
            |                      +---------+ \ /      +---+  +---+
   <input>--+               AM^2<--|amp^2 det|  X       |add|->|div|--<FM-out>
            |                      +---------+ / \      +---+  +---+
            |                           |     /   \       |-
            |                           *-->--     \      |
            |  +----------+  +-------+  |  +----+  +---+  |
            +->|*cos(2kHz)|->|FIR-LPF|--*->|z^-1|->|mul|--+
               +----------+  +-------+     +----+  +---+

   The cosine-signal is simply a sequence of 1,0,-1,0 , and the sine-
   signal is a sequence of 0,1,0,-1 values because the frequency of
   these is exactly a fouth of the sample-rate. The values multiplied by
   zero need not be evaluated by  the FIR-filter, what reduces the
   calculation-effort to one half. Taps of the FIR-chain which are to
   be multiplied with the same coefficient (possible due to the
   symmetry of the pulse-response) are first added or subtracted,
   so again reducing the number of multiplications to one half.

   NOTE: incnt must be a multiple of 2 to operate properly !
	 output range is determined by asntab, which is set up by
	 set_modem_param. This table also performs the arcsin-correction
	 and clips values that are beyond the given deviation.
*/
void fm_demod(char *smplin, int incnt, char *decout)
{
  static SHORT int firbuf[32];	/* buffer holding the input-values */
/*  static int inptr = 0;	/ pointer for data-entry into firbuf */
/*  static int rdptr = 0;	/ pointer for data-output from firbuf */
  static int neg = 0;		/* flag changing every 2 sample-points */
  static int px, py, qx, qy;    /* the filtered output-results */
  static int pamp, qamp, pfrq, qfrq;
/*
  static int n = 0;
  int smplptr = 0;
*/

  while (incnt >= 2) {

    /* shift buffer "left" by 2 : copy firbuf[2..17] to firbuf[0..15] */
    memcpy(firbuf, firbuf+2, (16*sizeof(int)));
    /* enter 2 new samle-points into buffer */
    firbuf[16] = *(unsigned char *)smplin++ - 128;
    firbuf[17] = *(unsigned char *)smplin++ - 128;
    incnt -= 2;
    
    /* do the first quarter : multiply with +0-0+0-0+0-0+0-0+ for px
       and 0-0+0-0+0-0+0-0+0 for py.
       2 sample-periodes later, multiply with 0-0+0 for px and -0+0- for py,
       what is done using the neg variable as flag. In this case, the result
       for frequency is negated (amplitude is always positive) */
    px =  (firbuf[0] + firbuf[16]) * fcoeff[0];
    px -= (firbuf[2] + firbuf[14]) * fcoeff[2];
    px += (firbuf[4] + firbuf[12]) * fcoeff[4];
    px -= (firbuf[6] + firbuf[10]) * fcoeff[6];
    px += (firbuf[8]) * fcoeff[8];
    px >>= 2;
    py =  (firbuf[15] - firbuf[1])  * fcoeff[1];
    py += (firbuf[3]  - firbuf[13]) * fcoeff[3];
    py += (firbuf[11] - firbuf[5])  * fcoeff[5];
    py += (firbuf[7]  - firbuf[9])  * fcoeff[7];
    py >>= 2;
/*
    dfft->src[smplptr].r = (neg) ? -px : px;
    dfft->src[smplptr].i = 0;
    smplptr++;
*/
/*    rdptr = (rdptr+1) & 31; */
    pamp = ((px*px + py*py) >> 8) + 1;
    pfrq = (px*qy - py*qx) / (pamp + qamp);

    /* do the second quarter : multiply with 0-0+0-0+0-0+0-0+0 for px,
       -0+0-0+0-0+0-0+0- for py */
    qx =  (firbuf[15+1] - firbuf[1+1])  * fcoeff[1];
    qx += (firbuf[3+1]  - firbuf[13+1]) * fcoeff[3];
    qx += (firbuf[11+1] - firbuf[5+1])  * fcoeff[5];
    qx += (firbuf[7+1]  - firbuf[9+1])  * fcoeff[7];
    qx >>= 2;
    qy = -(firbuf[0+1] + firbuf[16+1]) * fcoeff[0];
    qy += (firbuf[2+1] + firbuf[14+1]) * fcoeff[2];
    qy -= (firbuf[4+1] + firbuf[12+1]) * fcoeff[4];
    qy += (firbuf[6+1] + firbuf[10+1]) * fcoeff[6];
    qy -= (firbuf[8+1]) * fcoeff[8]; 
    qy >>= 2;
/*
    dfft->src[smplptr].r = (neg) ? -qx : qx;
    dfft->src[smplptr].i = 0;
    smplptr++;
*/
/*    rdptr = (rdptr+1) & 31; */
    qamp = ((qx*qx + qy*qy) >> 8) + 1;
    qfrq = (px*qy - py*qx) / (qamp + pamp);

    /* asntab gives values from minval to maxval for given deviation */
    *decout = (unsigned char)asntab[(pfrq+qfrq+256) & 511];
/*
    printf("%3d %5d %5d\n", n, (px*qy)/1024, (py*qx)/1024);
    printf("%3d %5d %5d\n", n, *decout, pamp+qamp);
    n++;
*/
    decout++;
    neg = ~neg;
  }
/*
  do_dfft(dfft);
  for (smplptr=0; smplptr<1024; smplptr++) {
    dx = dfft->dest[smplptr].r;
    dy = dfft->dest[smplptr].i;
    printf("%4.1f %3.1f\n", smplptr * 3.906, 20*log10(sqrt(dx*dx + dy*dy))); 
  }
*/
}


/* Demodulate the samples taken with 9.6kHz to an amplitude sampled with 4.8kHz.
   This done with a synchronous demodulator :

               +------------+  +-------+  +------+
            +->|*sin(2.4kHz)|->|FIR-LPF|->|square|-+
            |  +------------+  +-------+  +------+ |
            |                                      |
            |                                      |
            |                                   +-----+  +-------+
   <input>--+                                   |adder|->|sq.root|--<AM-out>
            |                                   +-----+  +-------+
            |                                      |
            |                                      |
            |  +------------+  +-------+  +------+ |
            +->|*cos(2.4kHz)|->|FIR-LPF|->|square|-+ 
               +------------+  +-------+  +------+

   The cosine-signal is simply a sequence of 1,0,-1,0 , and the sine-
   signal is a sequence of 0,1,0,-1 values because the frequency of
   these is exactly a fouth of the sample-rate. The values multiplied by
   zero need not be evaluated by  the FIR-filter, what reduces the
   calculation-effort to one half.

   NOTE: incnt must be a multiple of 2 to operate properly !
	 output range is determined by sqrt_lo and sqrt_hi, which
	 are set up by set_modem_param.
*/
void am_demod(char *smplin, int incnt, char *decout)
{
  static SHORT int firbuf[32];	/* buffer holding the input-values */
/*  static int inptr = 0;	/ pointer for data-entry into firbuf */
/*  static int rdptr = 0;	/ pointer for data-output from firbuf */
  static int neg = 0;		/* flag changing every 2 sample-points */
  static int px, py, qx, qy;    /* the filtered output-results */
  static int pamp, qamp;
/*
  static int n = 0;
  int smplptr = 0;
*/

  while (incnt >= 2) {

    /* shift buffer "left" by 2 : copy firbuf[2..17] to firbuf[0..15] */
    memcpy(firbuf, firbuf+2, (16*sizeof(int)));
    /* enter 2 new samle-points into buffer */
    firbuf[16] = *(unsigned char *)smplin++ - 128;
    firbuf[17] = *(unsigned char *)smplin++ - 128;
    incnt -= 2;
    
    /* do the first quarter : multiply with +0-0+0-0+0-0+0-0+ for px
       and 0-0+0-0+0-0+0-0+0 for py.
       2 sample-periodes later, multiply with 0-0+0 for px and -0+0- for py,
       what is done using the neg variable as flag. In this case, the result
       for frequency is negated (amplitude is always positive) */
    px =  (firbuf[0] + firbuf[16]) * fcoeff[0];
    px -= (firbuf[2] + firbuf[14]) * fcoeff[2];
    px += (firbuf[4] + firbuf[12]) * fcoeff[4];
    px -= (firbuf[6] + firbuf[10]) * fcoeff[6];
    px += firbuf[8] * fcoeff[8];
    px >>= 2;
    py =  (firbuf[15] - firbuf[1])  * fcoeff[1];
    py += (firbuf[3]  - firbuf[13]) * fcoeff[3];
    py += (firbuf[11] - firbuf[5])  * fcoeff[5];
    py += (firbuf[7]  - firbuf[9])  * fcoeff[7];
    py >>= 2;
/*
    dfft->src[smplptr].r = (neg) ? -px : px;
    dfft->src[smplptr].i = 0;
    smplptr++;
*/
/*    rdptr = (rdptr+1) & 31; */
    pamp = (px*px + py*py);

    /* do the second quarter : multiply with 0-0+0-0+0-0+0-0+0 for px,
       -0+0-0+0-0+0-0+0- for py */
    qx =  (firbuf[15+1] - firbuf[1+1])  * fcoeff[1];
    qx += (firbuf[3+1]  - firbuf[13+1]) * fcoeff[3];
    qx += (firbuf[11+1] - firbuf[5+1])  * fcoeff[5];
    qx += (firbuf[7+1]  - firbuf[9+1])  * fcoeff[7];
    qx >>= 2;
    qy = -(firbuf[0+1] + firbuf[16+1]) * fcoeff[0];
    qy += (firbuf[2+1] + firbuf[14+1]) * fcoeff[2];
    qy -= (firbuf[4+1] + firbuf[12+1]) * fcoeff[4];
    qy += (firbuf[6+1] + firbuf[10+1]) * fcoeff[6];
    qy -= firbuf[8+1] * fcoeff[8]; 
    qy >>= 2;
/*
    dfft->src[smplptr].r = (neg) ? -qx : qx;
    dfft->src[smplptr].i = 0;
    smplptr++;
*/
/*    rdptr = (rdptr+1) & 31; */
    qamp = (qx*qx + qy*qy + pamp) / 6400;

    if (qamp >= 65535)
      *decout = sqrt_hi[2047];
    else if (qamp >= 2047)
      *decout = (unsigned char)sqrt_hi[qamp >> 5];
    else
      *decout = (unsigned char)sqrt_lo[qamp];

/*
    printf("%3d %5d %5d\n", n, (px*px)/1024, (py*py)/1024);
    printf("%3d %5d %5d\n", n, *decout, qamp);
    n++;
*/
    decout++;
    neg = ~neg;
  }
}

/* Encode a frequency sampled at 4kHz to an AF-signal sampled at 8kHz.
   This is done by shifting the phase by the appropriate value for each
   sample.
*/ 
void fm_modulate(char *codin, int incnt, char *smplout)
{
  static int phs = 0;

  while (incnt-- > 0) {
    phs += 0x10000 + fmphsinc[*(unsigned char *)codin++];
    phs &= 0x3ffff;
    *smplout++ = (char)sintab[phs >> 8];
    phs += 0x10000 + fmphsinc[*(unsigned char *)codin++];
    phs &= 0x3ffff;
    *smplout++ = (char)sintab[phs >> 8];
  }
}

/* Encode an amplitude sampled at 4.8kHz to an AF-signal sampled at 9.6kHz.
   This is done by multiplying the amplitude-values with a sampled sine-
   function represented by a 0,1,0,-1 sequence.
*/
void am_modulate(char *codin, int incnt, char *smplout)
{
  static int neg = 0;

  while (incnt-- > 0) {
    if (neg) {
      *smplout++ = amreal[*(unsigned char *)codin++];
      *smplout++ = 128; /*amimag[*codin++]*/ 
    } else {
      *smplout++ = amreal[*(unsigned char *)codin++] ^ 0xff;
      *smplout++ = 128; /*amimag[*codin++] ^ 0xff*/
    }
    neg = ~neg;
  }
}