/* intercomd.cpp
 *
 * Copyright (C) DFS Deutsche Flugsicherung (2004, 2005). 
 * All Rights Reserved.
 * Author: Andre Adrian
 *
 * Voice over IP Intercom with Telephone conference and Acoustic Echo
 * Cancellation using unicast RTP messages (RFC3550, RFC3551)
 *
 * Attention. This source code is for Linux only! You need:
 *  same endian for CPU and soundcard for 16bit PCM audio sample
 *  Open Source Sound (OSS) support
 *  ALSA Sound support for hardware (2-channel) AEC
 * 
 * Format Sourcecode:
indent -kr -i2 -nlp -ci2 -l72 -lc72 -nut intercomd.cpp
 *
 * global Replace (one replace per line only!)
sed -i "s/PIT_MIN/G729_PIT_MIN/" *.h *.cpp 
 *
 */

const char VERSION[] = "0.4.1";

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <stdarg.h>
#include <syslog.h>

/* Error handling */
#include <assert.h>
#include <errno.h>
extern int errno;

/* low level io */
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <sys/select.h>
#include <sys/time.h>

/* Socket io */
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>

/* IETF iLBC codec */
extern "C" {
  #include "ilbc/iLBC_define.h"
  #include "ilbc/iLBC_encode.h"
  #include "ilbc/iLBC_decode.h"
}

/* ITU-T G.711 codec */ 
extern "C" {
  #include "g711/g711.h"
}

/* 3GPP GSM codec */ 
extern "C" {
  #include "gsm/private.h"
  #include "gsm/gsm.h"
  #include "gsm/rpeltp.h"
}

/* ITU-T G.726 codec */ 
extern "C" {
  #include "g726/g726.h"
  #include "g726/g726_rfc3551.h"
} 

/* Speex codec */
extern "C" {
  #include <speex/speex.h>
  /* Note: if you use the RPM speex-devel on SuSE 9.1 change the
   * header file above from <speex/speex.h> to <speex.h> */
}

#include "sinuston.h"

typedef unsigned char Byte;
typedef short Word16;

#ifdef EFRSTUB
/* EFR stubs - because of function name conflict in EFR and G.729 */
class EFR {
public:
  int initEncode(int dtx_mode_) {
    return 0;
  };
  int encode(              /* (o) len in byte of encoded_data */
    Byte * encoded_data,        /* (o) The encoded bytes */
    Word16 * data               /* (i) The signal block to encode */
    ) {
    return 0;
  };
  int initDecode() {
    return 0;
  };
  int decode(              /* (o) Number of decoded samples or 0 for error */
    Word16 * decoded_data,      /* (o) Decoded signal block */
    Byte * encoded_data,        /* (i) Encoded bytes */
    Word16 mode                 /* (i) 0=PL, 1=Normal */
    ) {
    return 0;
  };
};
#else
  /* 3GPP GSM-EFR codec */
#include "efr/typedef.h"
#include "efr/efr.h"
#endif

#ifdef G729STUB
/* G.729 stubs - because of function name conflict in EFR and G.729 */
class G729 {
public:
  int initEncode(int dummy) {
    return 0;
  };

  int encode(             /* len in byte of encoded_data */
    Byte * encoded_data,        /* (o) The encoded bytes */
    Word16 * data               /* (i) The signal block to encode */
    ) {
    return 0;
  };
  int initDecode() {
    return 0;
  };

  int decode(             /* (o) Number of decoded samples */
    Word16 * decoded_data,      /* (o) Decoded signal block */
    Byte * encoded_data,        /* (i) Encoded bytes */
    Word16 mode                 /* (i) 0=PL, 1=Normal */
    ) {
    return 0;
  };
};
#else
  /* ITU-T G.729 codec */
#include "g729/typedef.h"
#include "g729/ld8k.h"
#include "g729/g729.h"
#endif


/* intercom */
#include "rtp.h"
#include "udp.h"
#include "tcp.h"
#include "aec.h"
#include "oss.h"
#include "cirbuf.h"
#include "intercomd.h"

/* Design Constants */
#define PARTNERS    5           /* maximum telephony partners */

/* End of Design Constants */
#define MAXPACKSIZE 160         /* maximum size of 20ms encoded audio */
#define ILBC_MODE   20
#define FRAMESIZE   (20*8*WIDEB*2)    /* compression frame size */
#define MTU	    1460        /* max. MTU size without Headers */

/* Globals */
int wdisplay = NO;  /* export aec.cpp */


/* audio */
static CIRBUF mic_cirbuf;
static CIRBUF spk_cirbuf;
static AEC aec;
static int channels = 1;
static int audio_wr_fd = -1;  // Audio Output (speaker)
static int packetlc = NO;
static SINUSTON sinuston;   // Erweiterung Christian Dorge
static int sinuson = 0;     // Sinustonaktivierung

/* network transmitting to partners */
static in_addr_t to_ip[PARTNERS];
static RTP to_rtp[PARTNERS];
static UDP to_udp[PARTNERS];
static iLBC_Enc_Inst_t enc_ilbc[PARTNERS];
static G726_state enc_g726[PARTNERS];
static EFR enc_efr[PARTNERS];
static G729 enc_g729[PARTNERS];
static gsm enc_gsm[PARTNERS];
static SpeexBits enc_spxbits[PARTNERS];
static void *enc_spx[PARTNERS];
static CIRBUF conf_cirbuf[PARTNERS];
static char tx_buf[PARTNERS][MTU];
static char *tx_pbuf[PARTNERS];
static int tx_frame[PARTNERS];
static int to_partners = 0;
static int telephone_conference = NO;
static int tx_frames = 4;
static int tx_payloadt = PT_iLBC;       /* default Codec */
static int tx_packsize = NO_OF_BYTES_20MS;
static unsigned long to_ssrc = 0;

/* network receiving from partners */
static in_addr_t from_ip[PARTNERS];
static unsigned long from_ssrc[PARTNERS];
static int from_cnt[PARTNERS];
static CIRBUF from_cirbuf[PARTNERS];
static iLBC_Dec_Inst_t dec_ilbc[PARTNERS];
static G726_state dec_g726[PARTNERS];
static EFR dec_efr[PARTNERS];
static G729 dec_g729[PARTNERS];
static gsm dec_gsm[PARTNERS];
static SpeexBits dec_spxbits[PARTNERS];
static void *dec_spx[PARTNERS];
static int from_partners = 0;
static int from_rx_pt[PARTNERS];
static RTP from_rtp[PARTNERS];

/*----------------------------------------------------------------*
*  iLBC Encoder interface function 
*---------------------------------------------------------------*/

short encode(                   /* (o) Number of bytes encoded */
  iLBC_Enc_Inst_t * iLBCenc_inst,       /* (i/o) Encoder instance */
  unsigned char *encoded_data,  /* (o) The encoded bytes */
  short *data                   /* (i) The signal block to encode */
  )
{
  float block[BLOCKL_MAX];
  int k;

  /* convert signal to float */

  for (k = 0; k < iLBCenc_inst->blockl; k++)
    block[k] = (float) data[k];

  /* do the actual encoding */

  iLBC_encode(encoded_data, block, iLBCenc_inst);

  return (iLBCenc_inst->no_of_bytes);
}

short spx_encode(               /* (o) Number of bytes encoded */
  SpeexBits *bits,
  void *enc_state,
  unsigned char *encoded_data,  /* (o) The encoded bytes */
  short *data                   /* (i) The signal block to encode */
  )
{
  float block[20*WIDEB*8];
  unsigned int k;

  /* convert signal to float */

  for (k = 0; k < sizeof(block) / sizeof(float); k++)
    block[k] = (float) data[k];

  /* do the actual encoding */

  speex_bits_reset(bits);
  speex_encode(enc_state, block, bits);
  int bytes = speex_bits_write(bits, (char *)encoded_data, 200);

	// RPM speex-devel 1.0.3 has different size then Speex 1.1.7
	if (bytes != SZ_SPX) {
		static int errcnt = 0;
		if (++errcnt > 0) {
			errcnt = -250;
  		syslog(LOG_WARNING, "%s:%d: %s: Error bytes = %d, Update Speex lib to 1.1\n", \
  		__FILE__, __LINE__, __PRETTY_FUNCTION__, bytes); \
		}
	}
	return SZ_SPX;
}  

void tx_buf_init(int i)
{
  tx_pbuf[i] = tx_buf[i];
  tx_frame[i] = 0;
}

int encode_any(int i, Byte * encoded_data, short *buf)
{
  static int once = 0;
  int len = 0;
  switch (tx_payloadt) {
  case PT_PCMA:
    len = 160;
    alaw_compress(len, buf, encoded_data);
    break;
  case PT_PCMU:
    len = 160;
    ulaw_compress(len, buf, encoded_data);
    break;
  case PT_GSM:
    gsm_encode(enc_gsm[i], buf, encoded_data);
    len = 33;
    break;
  case PT_EFR:
    len = enc_efr[i].encode(encoded_data, buf);
    break;
  case PT_G729:
    /* G.729 is a 10ms per frame codec */
    enc_g729[i].encode(encoded_data, buf);
    len = 2 * enc_g729[i].encode(encoded_data + 10, buf + 80);
    break;
  case PT_iLBC:
    len = encode(&enc_ilbc[i], encoded_data, buf);
    break;
  case PT_SPX:
    len = spx_encode(&enc_spxbits[i], enc_spx[i], encoded_data, buf);
    if (!once) {
      printf("speex encode len = %d\n", len);
      once = 1;
    }
    break;
  case PT_G726:
    len = g726_encode(&enc_g726[i], encoded_data, buf);
    break;
  }
  return len;
}

static int packet_loss = 0;

int audio_read(int audio_rd_fd)
{
  /* fat software interrupt routine: read audio, send UDP packets */

  short mic_buf[FRAGSIZE / 2];
  if (1 == channels) {
    size_t len = read(audio_rd_fd, mic_buf, FRAGSIZE);
    return_if(len != FRAGSIZE, ERROR);

    if (0 == to_partners) {
      /* start assembling send packets only if we have a target */
      return OKAY;
    }

    short spk_buf[FRAGSIZE / 2];
    spk_cirbuf.pop((char *) spk_buf, FRAGSIZE);

    /* Acoustic Echo Cancellation - using software buffers */
    int i;
    for (i = 0; i < FRAGSIZE / 2; ++i) {
      mic_buf[i] = aec.doAEC(mic_buf[i], spk_buf[i]);
    }
  } else {
    short mic2_buf[FRAGSIZE];
    size_t len = read(audio_rd_fd, mic2_buf, 2 * FRAGSIZE);
    return_if(len != 2 * FRAGSIZE, ERROR);

    if (0 == to_partners) {
      /* start assembling send packets only if we have a target */
      return OKAY;
    }

    /* Acoustic Echo Cancellation - using hardware audio mixer */
    int i;
    for (i = 0; i < FRAGSIZE / 2; ++i) {
      mic_buf[i] = aec.doAEC(mic2_buf[2 * i], mic2_buf[2 * i + 1]);
    }
  }

  int ret = mic_cirbuf.push((char *) mic_buf, FRAGSIZE);
  if (ret < 0) {
    syslog(LOG_WARNING, "mic_cirbuf.push overrun\n");
  }

  if (mic_cirbuf.getlen() >= FRAMESIZE) {
    /* My RFC3551 interpretation: Only one RTP Header for packets 
     * with a number of frames */
    int i;
    for (i = 0; i < PARTNERS; ++i) {
      if (to_ip[i] && 0 == tx_frame[i]) {
        to_rtp[i].reset_csrc();
        
        /* add ssrc from partners in csrc field */
        if (telephone_conference) {
          int j;
          for (j = 0; j < PARTNERS; ++j) {
            /* do not mix in origin */
            if (from_ssrc[j] && (to_ip[i] != from_ip[j])) {     
              to_rtp[i].add_csrc(from_ssrc[j]);
            }
          }
        }
        /* put RTP header */
        tx_pbuf[i] = RTP_network_copy(tx_pbuf[i], &to_rtp[i]);
      }
    }

    /* put payload (audio) */
    short micbuf[FRAMESIZE / 2];
    mic_cirbuf.pop((char *) micbuf, FRAMESIZE);

    if (telephone_conference) {
      /* telephone conference mix - everybody from/to everybody else */
      short from_buf[PARTNERS][FRAMESIZE / sizeof(short)];
      short sum_buf[FRAMESIZE / sizeof(short)];
      int i, k;
      unsigned int j;

      /* get audio from other partners */
      for (i = 0; i < PARTNERS; ++i) {
        conf_cirbuf[i].pop((char *) from_buf[i], FRAMESIZE);
      }

      for (i = 0; i < PARTNERS; ++i) {
        if (to_ip[i]) {
          for (j = 0; j < FRAMESIZE / sizeof(short); ++j) {
            /* mix */
            long sum = micbuf[j];
            for (k = 0; k < PARTNERS; ++k) {
              if (to_ip[i] != from_ip[k]) {     /* do not mix in origin */
                sum += from_buf[k][j];
              }
            }
            /* clip */
            if (sum > 32767) {
              sum_buf[j] = 32767;
            } else if (sum < -32767) {
              sum_buf[j] = -32767;
            } else {
              sum_buf[j] = sum;
            }
          }
          /* do encoding (audio compression) */
          Byte encoded_data[MAXPACKSIZE];
          int len = encode_any(i, encoded_data, sum_buf);

          /* distribute to transmit buffers */
          memcpy(tx_pbuf[i], encoded_data, len);
          tx_pbuf[i] += len;
          assert(tx_pbuf[i] - tx_buf[i] <= MTU);
        }
      }
    } else {
      /* intercom conference mixing - central node from/to other nodes */
      /* do encoding (audio compression) */
      Byte encoded_data[MAXPACKSIZE];
      int len = encode_any(0, encoded_data, micbuf);

      /* distribute to transmit buffers */
      int i;
      for (i = 0; i < PARTNERS; ++i) {
        if (to_ip[i]) {
          memcpy(tx_pbuf[i], encoded_data, len);
          tx_pbuf[i] += len;
          assert(tx_pbuf[i] - tx_buf[i] <= MTU);
        }
      }
    }

    /* transmit data packet(s) */
    for (i = 0; i < PARTNERS; ++i) {
      if (to_ip[i] && ++tx_frame[i] >= tx_frames) {

        if (random() >= packet_loss) {  // simulate packet loss
          to_udp[i].send(tx_buf[i], tx_pbuf[i] - tx_buf[i]);
        }

        /* prepare next go */
        tx_buf_init(i);
        to_rtp[i].next(tx_frames * 20 * 8);
      }
    }
  }
  return OKAY;
}

void partner_timeout()
{
  /* Delete old from_ssrc[] entries - this is not very quick! */
  int i;
  for (i = 0; i < PARTNERS; ++i) {
    if (from_ssrc[i] && from_cnt[i] == 0) {
      char s[20];
      print_gui("d %s\n", iptoa(s, from_ip[i]));
      from_ssrc[i] = 0;
      from_ip[i] = 0;
      from_rx_pt[i] = -1;
      from_rtp[i].reset_csrc();
      --from_partners;
      if (0 == from_partners) {
        /* no more demand for PCM out */
        close(audio_wr_fd);
        audio_wr_fd = -1;
      }
    }
    from_cnt[i] = 0;
  }
}


int partner_lookup(unsigned long ssrc, in_addr_t ip)
{
  /* search */
  int i;
  for (i = 0; i < PARTNERS; ++i) {
    if (from_ssrc[i] == ssrc) {
      ++from_cnt[i];
      return i;                 /* old entry */
    }
  }
  /* add new entry */
  for (i = 0; i < PARTNERS; ++i) {
    if (0 == from_ssrc[i]) {
      if (0 == from_partners) {
        spk_cirbuf.init();
      }
      from_ssrc[i] = ssrc;
      from_ip[i] = ip;
      from_cnt[i] = 1;

      initDecode(dec_ilbc + i, ILBC_MODE, 1);
      enc_efr[i].initDecode();
      enc_g729[i].initDecode();
      rpeltp_delete(dec_gsm[i]);
      dec_gsm[i] = rpeltp_init();
      speex_bits_destroy(dec_spxbits + i);
      speex_decoder_destroy(dec_spx[i]);
      speex_bits_init(dec_spxbits + i);
      dec_spx[i] = speex_decoder_init(&speex_wb_mode);
      int value = SPX_ENH;
      speex_decoder_ctl(dec_spx[i], SPEEX_SET_ENH, &value);

      from_cirbuf[i].init();
      conf_cirbuf[i].init();
      ++from_partners;
      char s[20];
      print_gui("r %s\n", iptoa(s, ip));
      
      printf("from_ssrc[%d] = %08lx\n", i, from_ssrc[i]);
      return i;
    }
  }
  return ERROR;
}

/*----------------------------------------------------------------*
*  iLBC Decoder interface function 
*---------------------------------------------------------------*/

short decode(                   /* (o) Number of decoded samples */
  iLBC_Dec_Inst_t * iLBCdec_inst,       /* (i/o) Decoder instance */
  short *decoded_data,          /* (o) Decoded signal block */
  unsigned char *encoded_data,  /* (i) Encoded bytes */
  short mode                    /* (i) 0=PL, 1=Normal */
  )
{
  int k;
  float decblock[BLOCKL_MAX], dtmp;

  /* check if mode is valid */

  if (mode < 0 || mode > 1) {
    printf("\nERROR - Wrong mode - 0, 1 allowed\n");
    exit(3);
  }

  /* do actual decoding of block */
  iLBC_decode(decblock, encoded_data, iLBCdec_inst, mode);

  /* convert to short */

  for (k = 0; k < iLBCdec_inst->blockl; k++) {
    dtmp = decblock[k];

    if (dtmp < MIN_SAMPLE)
      dtmp = MIN_SAMPLE;
    else if (dtmp > MAX_SAMPLE)
      dtmp = MAX_SAMPLE;
    decoded_data[k] = (short) dtmp;
  }

  return (iLBCdec_inst->blockl);
}

short spx_decode(               /* (o) Number of decoded samples */
  SpeexBits *bits,
  void *dec_state,
  short *decoded_data,          /* (o) Decoded signal block */
  unsigned char *encoded_data,  /* (i) Encoded bytes */
  short mode                    /* (i) 0=PL, 1=Normal */
  )
{
  unsigned int k;
  float decblock[20*WIDEB*8];

  /* do actual decoding of block */
  if (bits)
    speex_bits_read_from(bits, (char *)encoded_data, SZ_SPX);
  speex_decode(dec_state, bits, decblock);

  /* convert to short */

  for (k = 0; k < sizeof(decblock) / sizeof(float); k++) {
    decoded_data[k] = (short)decblock[k];
  }

  return 20*WIDEB*8;
}

void audio_write()
{
  int i;
  unsigned int j;
  short from_buf[PARTNERS][FRAGSIZE / sizeof(short)];
  short sum_buf[FRAGSIZE / sizeof(short)];
  int playout = 0;      // is 1 if we have audio, not silence

  /* get audio */
  for (i = 0; i < PARTNERS; ++i) {
    int ret = from_cirbuf[i].pop((char *) from_buf[i], FRAGSIZE);
    if (OKAY == ret) playout = 1;
    if (packetlc && ERROR == ret && from_ssrc[i] && audio_wr_fd >= 0) {
      // printf("audio_write() PLC ch=%d ret=%d \n", i, ret);

      /* do Packet Loss Concealment */
      short decoded_data[FRAMESIZE / 2];
      int len;
      switch (from_rx_pt[i]) {
      case PT_iLBC:
        len = decode(dec_ilbc + i, decoded_data, NULL, 0);
        from_cirbuf[i].push((char *) decoded_data, 2 * len);
        from_cirbuf[i].pop((char *) from_buf[i], FRAGSIZE);
        playout = 1;
        break;
      case PT_SPX:
        Byte spx_dummy[SZ_SPX];
        memset(spx_dummy, 0, SZ_SPX);
        len = spx_decode(NULL, dec_spx[i], 
         decoded_data, spx_dummy, 1);
        from_cirbuf[i].push((char *) decoded_data, 2 * len);
        from_cirbuf[i].pop((char *) from_buf[i], FRAGSIZE);
        playout = 1;
        break;
      case PT_EFR:
        Byte efr_dummy[31];
        memset(efr_dummy, 0, 31);
        efr_dummy[0] = 0xC0;
        len = dec_efr[i].decode(decoded_data, efr_dummy, 0);
        from_cirbuf[i].push((char *) decoded_data, 2 * len);
        from_cirbuf[i].pop((char *) from_buf[i], FRAGSIZE);
        playout = 1;
        break;
      case PT_G729:
        Byte g729_dummy[20];
        memset(g729_dummy, 0, 20);
        len = dec_g729[i].decode(decoded_data, g729_dummy, 0);
        len +=
          dec_g729[i].decode(decoded_data + 80, g729_dummy + 10, 0);
        from_cirbuf[i].push((char *) decoded_data, 2 * len);
        from_cirbuf[i].pop((char *) from_buf[i], FRAGSIZE);
        playout = 1;
        break;
      }
    }
  }

  // if (!playout) return; // knistern with AD1985
  
  // audio mixing with sinuston
  for (j = 0; j < FRAGSIZE / sizeof(short); ++j) {
    /* mix */
    long sum = 0;
    for (i = 0; i < PARTNERS; ++i) {
      sum += from_buf[i][j];
    }
    /* clip */
    if (sum > 32767) {
      sum = 32767;
    } else if (sum < -32767) {
      sum = -32767;
    }
    if (sinuson && playout) {
      // Sinuston dazumischen 11jul2005 adrian
      sum = sinuston.mischen(sum);
    }      
    sum_buf[j] = sum; 
  }

  if (1 == channels) {
    if (from_partners > 0) {
      /* save for 1-channel AEC */
      int ret = spk_cirbuf.push((char *) sum_buf, FRAGSIZE);
      if (ret < 0) {
        /* syslog(LOG_WARNING, "spk_cirbuf.push overrun\n"); */
      }
    }
    if (audio_wr_fd < 0) {
      /* late audio write open */
      audio_wr_fd = audio_init("/dev/dsp", channels, O_WRONLY);
    }
    if (audio_wr_fd >= 0) {
      write(audio_wr_fd, sum_buf, FRAGSIZE);
    }
  } else {
    short sum2_buf[FRAGSIZE];
    int i;
    for (i = 0; i < FRAGSIZE / 2; ++i) {
      sum2_buf[2 * i] = 0;              /* left channel nothing */
      sum2_buf[2 * i + 1] = sum_buf[i]; /* right channel spk */
    }
    if (audio_wr_fd < 0) {
      /* late audio write open */
      audio_wr_fd = audio_init("/dev/dsp", channels, O_WRONLY);
    }
    if (audio_wr_fd >= 0) {
      write(audio_wr_fd, sum2_buf, 2 * FRAGSIZE);
    }
  }
}

static unsigned short rtp_port = 5004;  /* see RFC3551 */

int udp_read(int udp_fd)
{
  /* software interrupt routine */
  char buf[MTU];
  struct sockaddr_in from_sock;
  socklen_t from_socklen = sizeof(sockaddr_in);
  RTP rtp_in;

  int len = recvfrom(udp_fd, buf, MTU, 0,
    (struct sockaddr *) &from_sock, &from_socklen);

  /* check Port number */
  in_addr_t from_ip = ntohl(from_sock.sin_addr.s_addr);
  in_port_t from_port = ntohs(from_sock.sin_port);
  return_if(from_port != rtp_port, ERROR);

  char *pbuf = RTP_host_copy(&rtp_in, buf);
  len -= (pbuf - buf);
  return_if(len < 0, ERROR);
  int rc = rtp_in.check();
  return_if(rc, ERROR);

  int partner = partner_lookup(rtp_in.getssrc(), from_ip);
  return_if(partner < 0, ERROR);

  /* Avoid Megaphone (station with itself) audio loop */
  if (to_ssrc == rtp_in.getssrc()) {
    // return ERROR;
  }
  
  /* Avoid telephone conference style audio loop (multipath) */
  int i;
  for (i = 0; i < PARTNERS; ++i) {
    /* drop packets with SSRC the same as an known CSRC */
    if (i != partner && from_rtp[i].find_csrc(rtp_in.getssrc())) {
      return ERROR;
    }
  }
  /* to avoid silence with audio loops: Only if above test passed
   * copy rtp_in */
  from_rtp[partner] = rtp_in;   /* die CSRC */

  int rx_frames = 0;
  int rx_pt = rtp_in.get_pt();
  /* detect number of frames and dynamic payload type from length */
  switch (len) {
  case 1 * 20:
    rx_frames = 1;
    break;                      /* G729 */
  case 1 * 33:
    rx_frames = 1;
    break;                      /* GSM */
  case 1 * 31:
    rx_frames = 1;
    rx_pt = PT_EFR;
    break;
  case 1 * 38:
    rx_frames = 1;
    rx_pt = PT_iLBC;
    break;
  case 1 * SZ_SPX:
    rx_frames = 1;
    rx_pt = PT_SPX;
    break;
  case 1 * 80:                 /* or 4*20 */
    switch (rx_pt) {
    case PT_G729:
      rx_frames = 4;
      break;
    default:
      rx_frames = 1;
      rx_pt = PT_G726;
      break;
    }
    break;
  case 1 * 160:                /* or 2*80 */
    switch (rx_pt) {
    case PT_PCMA:
    case PT_PCMU:
      rx_frames = 1;            /* G711 */
      break;
    default:
      rx_frames = 2;            /* G726 32kbps */
      rx_pt = PT_G726;
      break;
    }
    break;

  case 2 * 20:
    rx_frames = 2;
    break;
  case 2 * 33:
    rx_frames = 2;
    break;
  case 2 * 31:
    rx_frames = 2;
    rx_pt = PT_EFR;
    break;
  case 2 * 38:
    rx_frames = 2;
    rx_pt = PT_iLBC;
    break;
  case 2 * SZ_SPX:
    rx_frames = 2;
    rx_pt = PT_SPX;
    break;
  case 2 * 160:                /* or 4*80 */
    switch (rx_pt) {
    case PT_PCMA:
    case PT_PCMU:
      rx_frames = 2;            /* G711 */
      break;
    default:
      rx_frames = 4;            /* G726 32kbps */
      rx_pt = PT_G726;
      break;
    }
    break;

  case 3 * 20:
    rx_frames = 3;
    break;
  case 3 * 33:
    rx_frames = 3;
    break;
  case 3 * 31:
    rx_frames = 3;
    rx_pt = PT_EFR;
    break;
  case 3 * 38:
    rx_frames = 3;
    rx_pt = PT_iLBC;
    break;
  case 3 * SZ_SPX:
    rx_frames = 3;
    rx_pt = PT_SPX;
    break;
  case 3 * 80:
    rx_frames = 3;
    rx_pt = PT_G726;
    break;
  case 3 * 160:
    rx_frames = 3;
    break;

  case 4 * 33:
    rx_frames = 4;
    break;
  case 4 * 31:
    rx_frames = 4;
    rx_pt = PT_EFR;
    break;
  case 4 * 38:
    rx_frames = 4;
    rx_pt = PT_iLBC;
    break;
  case 4 * SZ_SPX:
    rx_frames = 4;
    rx_pt = PT_SPX;
    break;
  case 4 * 160:
    rx_frames = 4;
    break;
  };
  return_if(0 == rx_frames, ERROR);

  from_rx_pt[partner] = rx_pt;
  for (i = 0; i < rx_frames; ++i) {
    /* do decoding (audio decompression) */
    short decoded_data[FRAMESIZE / 2];
    switch (rx_pt) {
    case PT_PCMA:
      len = 160;
      alaw_expand(len, (Byte *) pbuf, decoded_data);
      pbuf += 160;
      break;
    case PT_PCMU:
      len = 160;
      ulaw_expand(len, (Byte *) pbuf, decoded_data);
      pbuf += 160;
      break;
    case PT_GSM:
      len = 160;
      gsm_decode(dec_gsm[partner], (Byte *) pbuf, decoded_data);
      pbuf += 33;
      break;
    case PT_EFR:
      len = dec_efr[partner].decode(decoded_data, (Byte *) pbuf, 1);
      pbuf += 31;
      break;
    case PT_G729:
      len = dec_g729[partner].decode(decoded_data, (Byte *) pbuf, 1);
      len +=
        dec_g729[partner].decode(decoded_data + 80,
        (Byte *) (pbuf + 10), 1);
      pbuf += 20;
      break;
    case PT_iLBC:
      len = decode(dec_ilbc + partner, decoded_data, (Byte *) pbuf, 1);
      pbuf += 38;
      break;
    case PT_SPX:
      len = spx_decode(dec_spxbits + partner, dec_spx[partner], 
       decoded_data, (Byte *) pbuf, 1);
      pbuf += SZ_SPX;
      break;
    case PT_G726:
      len =
        g726_decode(dec_g726 + partner, decoded_data, (Byte *) pbuf, 1);
      pbuf += 80;
      break;
    }
    return_if(len != FRAMESIZE / 2, ERROR);
    
    int ret = from_cirbuf[partner].push((char *) decoded_data, 2 * len);
    if (ret < 0) {
      syslog(LOG_WARNING, "from_cirbuf[%d].push overrun=%d\n", partner,
        ret / FRAGSIZE);
    }
    if (telephone_conference) {
      ret = conf_cirbuf[partner].push((char *) decoded_data, 2 * len);
      if (ret < 0) {
        /* syslog(LOG_WARNING, "conf_cirbuf[%d].push overrun=%d\n", 
           partner, ret/FRAGSIZE); */
      }
    }
  }
  return OKAY;
}

void spx_encoder_init(int i)
{
  speex_bits_init(enc_spxbits + i);
  enc_spx[i] = speex_encoder_init(&speex_wb_mode);
  int value = SPX_QUALITY;
  speex_encoder_ctl(enc_spx[i], SPEEX_SET_QUALITY, &value);
  value = SPX_BITRATE;
  speex_encoder_ctl(enc_spx[i], SPEEX_SET_BITRATE, &value);
  value = SPX_COMPLEXITY;
  speex_encoder_ctl(enc_spx[i], SPEEX_SET_COMPLEXITY, &value);
}

// user command
void command(char *cmd, int udp_fd)
{
  /* delete special characters like \r, \n */
  unsigned int i;
  for (i = 0; i < strlen(cmd); ++i) {
    if (cmd[i] < ' ') {         /* hack: assume ASCII coding */
      cmd[i] = 0;
      break;
    }
  }
  in_addr_t ip;
  switch (cmd[0]) {
  default:
    printf("voipconf commands:\n"
      "c IP-Adress           - connect to IP-Adress\n"
      "h IP-Adress           - hang-up IP-Adress\n\n");
    fflush(stdout);
    break;
  case 'p':
    /* do nothing */
    break;
  case 'c':
    ip = atoip(cmd + 2);
    for (i = 0; i < PARTNERS; ++i) {
      if (0 == to_ip[i]) {
        if (0 == to_partners) {
          mic_cirbuf.init();
        }
        tx_buf_init(i);
        to_ip[i] = ip;
        to_rtp[i].init(tx_payloadt, to_ssrc);
        to_udp[i].send_init(cmd + 2, rtp_port, udp_fd);
        switch (tx_payloadt) {
        case PT_EFR:
          enc_efr[i].initEncode(0);
          break;
        case PT_G729:
          enc_g729[i].initEncode(0);
          break;
        case PT_iLBC:
          initEncode(enc_ilbc + i, ILBC_MODE);
          break;
        case PT_GSM:
          rpeltp_delete(enc_gsm[i]);
          enc_gsm[i] = rpeltp_init();
          break;
        case PT_SPX:
          speex_bits_destroy(enc_spxbits + i);
          speex_encoder_destroy(enc_spx[i]);
          spx_encoder_init(i);
          break;
        }
        ++to_partners;
        break;
      }
    }
    break;
  case 'h':
    ip = atoip(cmd + 2);
    for (i = 0; i < PARTNERS; ++i) {
      if (ip == to_ip[i]) {
        to_ip[i] = 0;
        to_udp[i].send_close();
        --to_partners;

        print_gui("%s\n", cmd); /* Tcl/Tk needs \n */
        break;
      }
    }
    break;
  }

  /* syslog(LOG_WARNING, "cmd=%s to_partners=%d\n", cmd, to_partners); */
}

#define CMDLEN	80

// local signalling from intercom.tcl
int gui_read(int gui_fd, int udp_fd)
{
  char cmd[CMDLEN];

  int len = read(gui_fd, cmd, CMDLEN);

  if (len <= 0) {
    syslog(LOG_WARNING, "gui_read() close\n");
    int ret = shutdown(gui_fd, SHUT_RDWR);
    assert_errno(ret >= 0);

    return -1;
  }

  command(cmd, udp_fd);

  return gui_fd;
}

static int gui_fd = -1;

int print_gui(const char *fmt, ...)
{
/* in fmt: Formatstring as printf  */
/* in ...: Parameter(s) as printf  */

  if (gui_fd >= 0) {
    char s[MTU];
    va_list ap;
    va_start(ap, fmt);
    (void) vsnprintf(s, MTU, fmt, ap);
    va_end(ap);

    int len = strlen(s);

    return write(gui_fd, s, len);
  } else {
    return ERROR;
  }
}

struct timeval difftimeval(struct timeval time1, struct timeval time2)
{
  struct timeval diff;

  diff.tv_usec = time1.tv_usec - time2.tv_usec;
  if (diff.tv_usec < 0) {
    diff.tv_usec += 1000000;
    time2.tv_usec += 1;
  }
  diff.tv_sec = time1.tv_sec - time2.tv_sec;

  return diff;
}

float dB2q(float dB)
{
  /* Dezibel to Ratio */
  return powf(10.0f, dB / 20.0f);
}
float q2dB(float q)
{
  /* Ratio to Dezibel */
  return 20.0f * log10f(q);
}

/* program main loop. OS Event handler */
int loop(int audio_rd_fd, int udp_fd, int gui_listen_fd)
{

  struct timeval timeout;
  fd_set read_fds;
  int max_fd = 64;              /* should be max(fd, ..) + 1 */
  static struct timeval last_partner_timeout;
  static struct timeval last_getambient;
  static struct timeval last_spkout;
  static long remainder_spkout = 0;
  struct timezone tz;
  static int mic_int, spk_int, time_int;
  
  openlog(NULL, LOG_PERROR, LOG_WARNING);

  gettimeofday(&last_partner_timeout, &tz);
  gettimeofday(&last_getambient, &tz);
  gettimeofday(&last_spkout, &tz);
  for (;;) {
    timeout.tv_sec = 0;
    timeout.tv_usec = FRAGTIME * 1000;
    FD_ZERO(&read_fds);
    FD_SET(audio_rd_fd, &read_fds);
    FD_SET(udp_fd, &read_fds);
    FD_SET(gui_listen_fd, &read_fds);
    if (gui_fd >= 0) {
      FD_SET(gui_fd, &read_fds);
    }
    // int ret = select(max_fd, &read_fds, NULL, NULL, &timeout);
    int ret = select(max_fd, &read_fds, NULL, NULL, NULL);
    assert_errno(ret >= 0);

    if (FD_ISSET(audio_rd_fd, &read_fds)) {
      audio_read(audio_rd_fd);
      ++mic_int;
    }
    if (FD_ISSET(udp_fd, &read_fds)) {
      udp_read(udp_fd);
    }
    if (FD_ISSET(gui_listen_fd, &read_fds)) {
      gui_fd = tcp_server_init2(gui_listen_fd);
    }
    if (gui_fd >= 0) {
      if (FD_ISSET(gui_fd, &read_fds)) {
        gui_fd = gui_read(gui_fd, udp_fd);
      }
    }
#if 0
    if ((mic_int % (5000 / FRAGTIME)) == 0) {
      printf("int: mic %d spk %d time %d\n", mic_int, spk_int,
        time_int);
    }
#endif

    struct timeval now, diff;
    if (packetlc) {
      gettimeofday(&now, &tz);
      diff = difftimeval(now, last_spkout);
      if (diff.tv_usec + remainder_spkout >= FRAGTIME * 1000) {
        ++time_int;
        last_spkout = now;
        /* Linux time resolution is more coarse (15ms) then mic 
         * time interval (4ms), therefore remainder calculation */
        remainder_spkout =
          diff.tv_usec + remainder_spkout - FRAGTIME * 1000;
        if (audio_wr_fd < 0) {
          /* new output: fill up Open Sound System (OSS) buffer 
           * to compensate Operation System Jitter */
          // audio_write();
        }
        audio_write();
        ++spk_int;
      }
    } else {
       /* because of problems with Intel ICH5/Analog Devices AD1985
        * not together with audio_read() */
      audio_write();
    }

    gettimeofday(&now, &tz);
    diff = difftimeval(now, last_partner_timeout);
    if (diff.tv_usec >= 160000) {       /* 2*PACKETDURATION in usec */
      last_partner_timeout = now;
      partner_timeout();
    }
#if 0
    gettimeofday(&now, &tz);
    diff = difftimeval(now, last_getambient);
    if (diff.tv_sec >= 2) {
      last_getambient = now;
      // if (to_partners > 0) {
        // float ambient = aec.getambient();
        // float ambientdB = q2dB(ambient / 32767.0f);
        // syslog(LOG_WARNING, "Ambient = %2.0f dB\n", ambientdB);
        float xfastdB = q2dB(aec.xfast / 32767.0f);
        float dfastdB = q2dB(aec.dfast / 32767.0f);
        float efastdB = q2dB(aec.efast / 32767.0f);
        float xslowdB = q2dB(aec.xslow / 32767.0f);
        float dslowdB = q2dB(aec.dslow / 32767.0f);
        syslog(LOG_WARNING, "ratio= %2.0f / %2.0f / %2.0f / %2.0f e= %2.0f\n", 
         dfastdB, xfastdB, dslowdB, xslowdB, efastdB);
      // }
    }
#endif    
  }
  return ERROR;
}

int main(int argc, char *argv[])
{
  int i;
  float ambient = 0.0f;
  float dB;

#if WIDEB==1      
  tx_payloadt = PT_iLBC;
  tx_packsize = SZ_iLBC;
  printf("Narrowband DFS VoIP Intercom %s\n", VERSION);
#else
  tx_payloadt = PT_SPX;
  tx_packsize = SZ_SPX;
  printf("Wideband DFS VoIP Intercom %s\n", VERSION);
#endif
  for (i = 1; i < argc && '-' == argv[i][0]; ++i) {
    switch (argv[i][1]) {
    case 'a':                  /* set Ambient (No Talking) Noise level */
      ambient = atof(argv[++i]);
      break;
    case 'b':                  /* No AES */
      dB = atof(argv[++i]);
      if (dB < 0.0f) {
        aec.setaes(dB2q(dB));
      } else {
        aec.setaes(0.0f);   // 0 is no AES
      }
      break;
    case 'e':  
      sinuson = 1;                 /* Sinuston 2040Hz zumischen */
      break;
    case 'k':                  /* packet loss concealment */
      packetlc = YES;
      break;
    case 'f':                  /* Frames */
      tx_frames = atoi(argv[++i]);
      if (tx_frames < 1)
        tx_frames = 1;
      if (tx_frames > 4)
        tx_frames = 4;
      break;
    case 'l':                  /* use hardware AEC and Line-in for microphone */
      channels = 2;
      break;
    case 'm':                  /* use hardware AEC and Mic-in for microphone */
      channels = 2;
      break;
    case 'p':                  /* RTP Portnumber */
      rtp_port = atoi(argv[++i]);
      rtp_port &= 0xFFFE;       /* RFC3551: RTP port has even port number */
      break;
    case 's':                  /* simulated Packet loss */
      packet_loss = (RAND_MAX / 100) * atoi(argv[++i]);
      break;
    case 't':                  /* Telephone conference call (true conference) */
      telephone_conference = YES;
      break;
    case 'w':                  /* visualize vector w */
      aec.openwdisplay();
      break;
#if WIDEB==1      
    case 'A':                  /* set G.711 A-law */
      tx_payloadt = PT_PCMA;
      tx_packsize = 160;        /* see RFC3551 */
      break;
    case 'U':                  /* set G.711 u-law */
      tx_payloadt = PT_PCMU;
      tx_packsize = 160;        /* see RFC3551 */
      break;
    case '6':                  /* set G.726 codec */
      tx_payloadt = PT_G726;
      tx_packsize = 80;         /* see RFC3551 */
      break;
    case 'G':                  /* set GSM codec */
      tx_payloadt = PT_GSM;
      tx_packsize = 33;         /* see RFC3551 */
      break;
    case 'E':                  /* set GSM-EFR codec */
      tx_payloadt = PT_EFR;
      tx_packsize = 31;         /* see RFC3551 */
      break;
    case '9':                  /* set G.729 codec */
      tx_payloadt = PT_G729;
      tx_packsize = 20;         /* see RFC3551 */
      break;
#else
    case 'S':                  /* set Speex codec */
      tx_payloadt = PT_SPX;
      tx_packsize = SZ_SPX;    /* 16.8kBit/s Wideband */
      break;
#endif    
    }
  }

  /* do -a option even if codec options follows */
  if (ambient != 0.0f) {
    aec.setambient(MAXPCM * dB2q(ambient));
  }

  /* init Sinuston fuer Transmitter */
  sinuston.frequenz(2040, WIDEB*8000, -24.0f);
  
  /* open Audio Receive */
  int audio_rd_fd = audio_init("/dev/dsp", channels, O_RDONLY);
  assert(audio_rd_fd >= 0);

  /* open Network Receive */
  int udp_fd = UDP_recv_init(rtp_port);
  assert(udp_fd >= 0);

  /* open Graphical User Interface as TCP server */
  int gui_listen_fd = tcp_server_init(4999);

  /* codec Initialization */
  int j;
  for (j = 0; j < PARTNERS; ++j) {
    /* iLBC */
    initEncode(enc_ilbc + j, ILBC_MODE);
    initDecode(dec_ilbc + j, ILBC_MODE, 1);

    /* GSM-EFR */
    enc_efr[j].initEncode(0);
    dec_efr[j].initDecode();

    /* G.729 */
    enc_g729[j].initEncode(0);
    dec_g729[j].initDecode();

    /* GSM */
    enc_gsm[i] = rpeltp_init();
    dec_gsm[i] = rpeltp_init();
    
    /* Speex */
    spx_encoder_init(j);
    
    speex_bits_init(dec_spxbits + j);
    dec_spx[j] = speex_decoder_init(&speex_wb_mode);  // Wideband
    int value = SPX_ENH;
    speex_decoder_ctl(dec_spx[j], SPEEX_SET_ENH, &value);
  }

  /* open Network Transmit Partner (Connections) */
  to_ssrc = random32(tx_payloadt);
  printf("to_ssrc = %08lx\n", to_ssrc);
  for (; i < argc; ++i) {
    if (0 == to_partners) {
      mic_cirbuf.init();
    }
    tx_buf_init(to_partners);
    to_ip[to_partners] = atoip(argv[i]);
    to_rtp[to_partners].init(tx_payloadt, to_ssrc);
    to_udp[to_partners].send_init(argv[i], rtp_port, udp_fd);
    ++to_partners;
  }

  loop(audio_rd_fd, udp_fd, gui_listen_fd);

  return OKAY;
}