/*
 *      CVSD (Continuously Variable Slope Delta modulation)
 *      conversion routines
 *
 *      The CVSD format is described in the MIL Std 188 113, which is
 *      available from http://bbs.itsi.disa.mil:5580/T3564
 *
 *	Copyright (C) 1996  
 *      Thomas Sailer (sailer@ife.ee.ethz.ch) (HB9JNX/AE4WA)
 *      Swiss Federal Institute of Technology, Electronics Lab
 *
 *	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., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 *
 * Change History:
 *
 * June 1, 1998 - Chris Bagwell (cbagwell@sprynet.com)
 *   Fixed compile warnings reported by Kjetil Torgrim Homme
 *   <kjetilho@ifi.uio.no>
 *
 *
 */

/* ---------------------------------------------------------------------- */

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

#ifdef HAVE_UNISTD_H
#include <unistd.h>	/* For SEEK_* defines if not found in stdio */
#endif

#include "cvsdfilt.h"
#include "st.h"
#include "libst.h"

/* ---------------------------------------------------------------------- */

#ifndef HAVE_MEMMOVE
#define memmove(dest,src,len) (bcopy((src),(dest),(len)))
#endif

/* ---------------------------------------------------------------------- */
/*
 * private data structures
 */

struct cvsd_common_state {
	unsigned overload;
	float mla_int;
	float mla_tc0;
	float mla_tc1;
	unsigned phase;
	unsigned phase_inc;
	float v_min, v_max;
};

struct cvsd_decode_state {
	float output_filter[DEC_FILTERLEN];
};

struct cvsd_encode_state {
	float recon_int;
	float input_filter[ENC_FILTERLEN];
};

struct cvsdpriv {
	struct cvsd_common_state com;
	union {
		struct cvsd_decode_state dec;
		struct cvsd_encode_state enc;
	} c;
	struct {
		unsigned shreg;
		unsigned mask;
		unsigned cnt;
	} bit;
	unsigned bytes_written;
	unsigned cvsd_rate;
	char swapbits;
};

/* ---------------------------------------------------------------------- */

float float_conv(fp1, fp2, n)
float *fp1;
float *fp2;
int n;
{
	float res = 0;
	for(; n > 0; n--)
		res += (*fp1++) * (*fp2++);
	return res;
}

/* ---------------------------------------------------------------------- */
/*
 * some remarks about the implementation of the CVSD decoder
 * the principal integrator is integrated into the output filter
 * to achieve this, the coefficients of the output filter are multiplied
 * with (1/(1-1/z)) in the initialisation code.
 * the output filter must have a sharp zero at f=0 (i.e. the sum of the
 * filter parameters must be zero). This prevents an accumulation of
 * DC voltage at the principal integration.
 */
/* ---------------------------------------------------------------------- */

static void cvsdstartcommon(ft)
ft_t ft;
{
	struct cvsdpriv *p = (struct cvsdpriv *) ft->priv;
	
	/* sanity check */
	if (sizeof(struct cvsdpriv) > PRIVSIZE)
		fail("struct cvsdpriv is too big (%d); change PRIVSIZE in st.h and recompile sox", sizeof(struct cvsdpriv));
	p->cvsd_rate = (ft->info.rate <= 24000) ? 16000 : 32000;
	ft->info.rate = 8000;
	ft->info.channels = 1;
	ft->info.size = WORD; /* make output format default to words */
	ft->info.style = SIGN2;
	p->swapbits = ft->swap;
	ft->swap = 0;
	/*
	 * initialize the decoder
	 */
	p->com.overload = 0x5;
	p->com.mla_int = 0;
	/*
	 * timeconst = (1/e)^(200 / SR) = exp(-200/SR)
	 * SR is the sampling rate
	 */
	p->com.mla_tc0 = exp((-200.0)/((float)(p->cvsd_rate)));
	/*
	 * phase_inc = 32000 / SR
	 */
	p->com.phase_inc = 32000 / p->cvsd_rate;
	/*
	 * initialize bit shift register
	 */
	p->bit.shreg = p->bit.cnt = 0;
	p->bit.mask = p->swapbits ? 0x80 : 1;
	/*
	 * count the bytes written
	 */
	p->bytes_written = 0;
	p->com.v_min = 1;
	p->com.v_max = -1;
	report("cvsd: bit rate %dbit/s, bits from %s\n", p->cvsd_rate,
	       p->swapbits ? "msb to lsb" : "lsb to msb");
}

/* ---------------------------------------------------------------------- */

void cvsdstartread(ft) 
ft_t ft;
{
	struct cvsdpriv *p = (struct cvsdpriv *) ft->priv;
	float *fp1;
	int i;
	
	cvsdstartcommon(ft);
	p->com.mla_tc1 = 0.1 * (1 - p->com.mla_tc0);
	p->com.phase = 0;
	/*
	 * initialize the output filter coeffs (i.e. multiply
	 * the coeffs with (1/(1-1/z)) to achieve integration
	 * this is now done in the filter parameter generation utility
	 */
	/*
	 * zero the filter 
	 */
	for(fp1 = p->c.dec.output_filter, i = DEC_FILTERLEN; i > 0; i--)
		*fp1++ = 0;
}

/* ---------------------------------------------------------------------- */

void cvsdstartwrite(ft) 
ft_t ft;
{
	struct cvsdpriv *p = (struct cvsdpriv *) ft->priv;
	float *fp1;
	int i;

	cvsdstartcommon(ft);
	p->com.mla_tc1 = 0.1 * (1 - p->com.mla_tc0);
	p->com.phase = 4;
	/*
	 * zero the filter 
	 */
	for(fp1 = p->c.enc.input_filter, i = ENC_FILTERLEN; i > 0; i--)
		*fp1++ = 0;
	p->c.enc.recon_int = 0;
}

/* ---------------------------------------------------------------------- */

void
cvsdstopwrite(ft)
ft_t ft;
{
	struct cvsdpriv *p = (struct cvsdpriv *) ft->priv;

	if (p->bit.cnt) {
		putc(p->bit.shreg, ft->fp);
		p->bytes_written++;
	}
	report("cvsd: min slope %f, max slope %f\n", 
	       p->com.v_min, p->com.v_max);	
}

/* ---------------------------------------------------------------------- */

void
cvsdstopread(ft)
ft_t ft;
{
	struct cvsdpriv *p = (struct cvsdpriv *) ft->priv;

	report("cvsd: min value %f, max value %f\n", 
	       p->com.v_min, p->com.v_max);
}

/* ---------------------------------------------------------------------- */

#undef DEBUG

#ifdef DEBUG
static struct {
	FILE *f1;
	FILE *f2;
	int cnt
} dbg = { NULL, NULL, 0 };
#endif

LONG cvsdread(ft, buf, nsamp) 
ft_t ft;
LONG *buf, nsamp;
{
	struct cvsdpriv *p = (struct cvsdpriv *) ft->priv;
	int done = 0;
	float oval;
	
#ifdef DEBUG
	if (!dbg.f1) {
		if (!(dbg.f1 = fopen("dbg1", "w")))
			fail("debugging");
		fprintf(dbg.f1, "\"input\"\n");
	}
	if (!dbg.f2) {
		if (!(dbg.f2 = fopen("dbg2", "w")))
			fail("debugging");
		fprintf(dbg.f2, "\"recon\"\n");
	}
#endif
	while (done < nsamp) {
		if (!p->bit.cnt) {
			p->bit.shreg = getc(ft->fp);
			if (feof(ft->fp))
				return done;
			p->bit.cnt = 8;
			p->bit.mask = p->swapbits ? 0x80 : 1;
		}
		/*
		 * handle one bit
		 */
		p->bit.cnt--;
		p->com.overload = ((p->com.overload << 1) | 
				   (!!(p->bit.shreg & p->bit.mask))) & 7;
		if (p->swapbits)
			p->bit.mask >>= 1;
		else
			p->bit.mask <<= 1;
		p->com.mla_int *= p->com.mla_tc0;
		if ((p->com.overload == 0) || (p->com.overload == 7))
			p->com.mla_int += p->com.mla_tc1;
		memmove(p->c.dec.output_filter+1, p->c.dec.output_filter,
			sizeof(p->c.dec.output_filter)-sizeof(float));
		if (p->com.overload & 1)
			p->c.dec.output_filter[0] = p->com.mla_int;
		else
			p->c.dec.output_filter[0] = -p->com.mla_int;
		/*
		 * check if the next output is due
		 */
		p->com.phase += p->com.phase_inc;
		if (p->com.phase >= 4) {
			oval = float_conv(p->c.dec.output_filter, 
					  (p->cvsd_rate < 24000) ? 
					  dec_filter_16 : dec_filter_32, 
					  DEC_FILTERLEN);
#ifdef DEBUG
			fprintf(dbg.f1, "%f %f\n", (double)dbg.cnt, 
				(double)p->com.mla_int);
			fprintf(dbg.f2, "%f %f\n", (double)dbg.cnt, 
				(double)oval);
			dbg.cnt++;
#endif		
			if (oval > p->com.v_max)
				p->com.v_max = oval;
			if (oval < p->com.v_min)
				p->com.v_min = oval;
			*buf++ = (oval * ((float)LONG_MAX));
			done++;
		}
		p->com.phase &= 3;
	}
	return done;
}

/* ---------------------------------------------------------------------- */

void
cvsdwrite(ft, buf, nsamp) 
ft_t ft;
LONG *buf, nsamp;
{
	struct cvsdpriv *p = (struct cvsdpriv *) ft->priv;
	int done = 0;
	float inval;

#ifdef DEBUG
	if (!dbg.f1) {
		if (!(dbg.f1 = fopen("dbg1", "w")))
			fail("debugging");
		fprintf(dbg.f1, "\"input\"\n");
	}
	if (!dbg.f2) {
		if (!(dbg.f2 = fopen("dbg2", "w")))
			fail("debugging");
		fprintf(dbg.f2, "\"recon\"\n");
	}
#endif
	for(;;) {
		/*
		 * check if the next input is due
		 */
		if (p->com.phase >= 4) {
			if (done >= nsamp)
				return;
			memmove(p->c.enc.input_filter+1, p->c.enc.input_filter,
				sizeof(p->c.enc.input_filter)-sizeof(float));
			p->c.enc.input_filter[0] = (*buf++) / 
				((float)LONG_MAX);
			done++;
		}
		p->com.phase &= 3;
		/* insert input filter here! */
		inval = float_conv(p->c.enc.input_filter, 
				   (p->cvsd_rate < 24000) ? 
				   (enc_filter_16[(p->com.phase >= 2)]) : 
				   (enc_filter_32[p->com.phase]), 
				   ENC_FILTERLEN);
		/*
		 * encode one bit
		 */
		p->com.overload = (((p->com.overload << 1) |
				    (inval >  p->c.enc.recon_int)) & 7);
		p->com.mla_int *= p->com.mla_tc0;
		if ((p->com.overload == 0) || (p->com.overload == 7))
			p->com.mla_int += p->com.mla_tc1;
		if (p->com.mla_int > p->com.v_max)
			p->com.v_max = p->com.mla_int;
		if (p->com.mla_int < p->com.v_min)
			p->com.v_min = p->com.mla_int;
		if (p->com.overload & 1) {
			p->c.enc.recon_int += p->com.mla_int;
			p->bit.shreg |= p->bit.mask;
		} else
			p->c.enc.recon_int -= p->com.mla_int;
		if ((++(p->bit.cnt)) >= 8) {
			putc(p->bit.shreg, ft->fp);
			p->bytes_written++;
			p->bit.shreg = p->bit.cnt = 0;
			p->bit.mask = p->swapbits ? 0x80 : 1;
		} else {
			if (p->swapbits)
				p->bit.mask >>= 1;
			else
				p->bit.mask <<= 1;
		}
		p->com.phase += p->com.phase_inc;
#ifdef DEBUG
		fprintf(dbg.f1, "%f %f\n", (double)dbg.cnt, (double)inval);
		fprintf(dbg.f2, "%f %f\n", (double)dbg.cnt, 
			(double)p->c.enc.recon_int);
		dbg.cnt++;
#endif	
	}
}

/* ---------------------------------------------------------------------- */
/*
 * DVMS file header
 */
struct dvms_header {
	char          Filename[14];
	unsigned      Id;
	unsigned      State;
	time_t        Unixtime;
	unsigned      Usender;
	unsigned      Ureceiver;
	ULONG	      Length;
	unsigned      Srate;
	unsigned      Days;
	unsigned      Custom1;
	unsigned      Custom2;
	char          Info[16];
	char          extend[64];
	unsigned      Crc;
};

#define DVMS_HEADER_LEN 120

/* ---------------------------------------------------------------------- */

static ULONG get32(p)
unsigned char **p;
{
	ULONG val = (((*p)[3]) << 24) | (((*p)[2]) << 16) | 
		(((*p)[1]) << 8) | (**p);
	(*p) += 4;
	return val;
}

static unsigned get16(p)
unsigned char **p;
{
	unsigned val = (((*p)[1]) << 8) | (**p);
	(*p) += 2;
	return val;
}

static void put32(p, val)
unsigned char **p;
ULONG val;
{
	*(*p)++ = val & 0xff;
	*(*p)++ = (val >> 8) & 0xff;
	*(*p)++ = (val >> 16) & 0xff;
	*(*p)++ = (val >> 24) & 0xff;
}

static void put16(p, val)
unsigned char **p;
unsigned val;
{
	*(*p)++ = val & 0xff;
	*(*p)++ = (val >> 8) & 0xff;
}

/* ---------------------------------------------------------------------- */

static void dvms_read_header(f, hdr)
FILE *f;
struct dvms_header *hdr;
{
	unsigned char hdrbuf[DVMS_HEADER_LEN];
	unsigned char *pch = hdrbuf;
	int i;
	unsigned sum;

	if (fread(hdrbuf, sizeof(hdrbuf), 1, f) != 1)
		fail("unable to read DVMS header\n");
	for(i = sizeof(hdrbuf), sum = 0; i > /*2*/3; i--) /* Deti bug */
		sum += *pch++;
	pch = hdrbuf;
	memcpy(hdr->Filename, pch, sizeof(hdr->Filename));
	pch += sizeof(hdr->Filename);
	hdr->Id = get16(&pch);
	hdr->State = get16(&pch);
	hdr->Unixtime = get32(&pch);
	hdr->Usender = get16(&pch);
	hdr->Ureceiver = get16(&pch);
	hdr->Length = get32(&pch);
	hdr->Srate = get16(&pch);
	hdr->Days = get16(&pch);
	hdr->Custom1 = get16(&pch);
	hdr->Custom2 = get16(&pch);
	memcpy(hdr->Info, pch, sizeof(hdr->Info));
	pch += sizeof(hdr->Info);
	memcpy(hdr->extend, pch, sizeof(hdr->extend));
	pch += sizeof(hdr->extend);
	hdr->Crc = get16(&pch);
	if (sum != hdr->Crc) 
		fail("DVMS header checksum error, read %u, calculated %u\n",
		     hdr->Crc, sum);
}

/* ---------------------------------------------------------------------- */

/*
 * note! file must be seekable
 */
static void dvms_write_header(f, hdr)
FILE *f;
struct dvms_header *hdr;
{
	unsigned char hdrbuf[DVMS_HEADER_LEN];
	unsigned char *pch = hdrbuf;
	unsigned char *pchs = hdrbuf;
	int i;
	unsigned sum;

	memcpy(pch, hdr->Filename, sizeof(hdr->Filename));
	pch += sizeof(hdr->Filename);
	put16(&pch, hdr->Id);
	put16(&pch, hdr->State);
	put32(&pch, hdr->Unixtime);
	put16(&pch, hdr->Usender);
	put16(&pch, hdr->Ureceiver);
	put32(&pch, hdr->Length);
	put16(&pch, hdr->Srate);
	put16(&pch, hdr->Days);
	put16(&pch, hdr->Custom1);
	put16(&pch, hdr->Custom2);
	memcpy(pch, hdr->Info, sizeof(hdr->Info));
	pch += sizeof(hdr->Info);
	memcpy(pch, hdr->extend, sizeof(hdr->extend));
	pch += sizeof(hdr->extend);
	for(i = sizeof(hdrbuf), sum = 0; i > /*2*/3; i--) /* Deti bug */
		sum += *pchs++;
	hdr->Crc = sum;
	put16(&pch, hdr->Crc);
	if (fseek(f, 0, SEEK_SET) < 0)
		fail("cannot write DVMS header, seek failed\n");
	if (fwrite(hdrbuf, sizeof(hdrbuf), 1, f) != 1)
		fail("cannot write DVMS header\n");
}

/* ---------------------------------------------------------------------- */

static void make_dvms_hdr(ft, hdr)
ft_t ft;
struct dvms_header *hdr;
{
	struct cvsdpriv *p = (struct cvsdpriv *) ft->priv;
	int len;

	memset(hdr->Filename, 0, sizeof(hdr->Filename));
	len = strlen(ft->filename);
	if (len >= sizeof(hdr->Filename))
		len = sizeof(hdr->Filename)-1;
	memcpy(hdr->Filename, ft->filename, len);
	hdr->Id = hdr->State = 0;
	hdr->Unixtime = time(NULL);
	hdr->Usender = hdr->Ureceiver = 0;
	hdr->Length = p->bytes_written;
	hdr->Srate = p->cvsd_rate/100;
	hdr->Days = hdr->Custom1 = hdr->Custom2 = 0;
	memset(hdr->Info, 0, sizeof(hdr->Info));
	len = strlen(ft->comment);
	if (len >= sizeof(hdr->Info))
		len = sizeof(hdr->Info)-1;
	memcpy(hdr->Info, ft->comment, len);
	memset(hdr->extend, 0, sizeof(hdr->extend));
}

/* ---------------------------------------------------------------------- */

void dvmsstartread(ft) 
ft_t ft;
{
	struct cvsdpriv *p = (struct cvsdpriv *) ft->priv;
	struct dvms_header hdr;

	dvms_read_header(ft->fp, &hdr);
	report("DVMS header of source file \"%s\":");
	report("  filename  \"%.14s\"",ft->filename);
        report("  id        0x%x", hdr.Filename);
	report("  state     0x%x", hdr.Id, hdr.State);
	report("  time      %s",ctime(&hdr.Unixtime)); /* ctime generates lf */
	report("  usender   %u", hdr.Usender);
	report("  ureceiver %u", hdr.Ureceiver);
	report("  length    %u", hdr.Length);
	report("  srate     %u", hdr.Srate);
	report("  days      %u", hdr.Days);
	report("  custom1   %u", hdr.Custom1);
	report("  custom2   %u", hdr.Custom2);
	report("  info      \"%.16s\"\n", hdr.Info);
	ft->info.rate = (hdr.Srate < 240) ? 16000 : 32000;
	report("DVMS rate %dbit/s using %dbit/s deviation %d%%\n", 
	       hdr.Srate*100, ft->info.rate, 
	       ((ft->info.rate - hdr.Srate*100) * 100) / ft->info.rate);
	cvsdstartread(ft);
	p->swapbits = 0;
}

/* ---------------------------------------------------------------------- */

void dvmsstartwrite(ft) 
ft_t ft;
{
	struct cvsdpriv *p = (struct cvsdpriv *) ft->priv;
	struct dvms_header hdr;
	
	cvsdstartwrite(ft);
	make_dvms_hdr(ft, &hdr);
	dvms_write_header(ft->fp, &hdr);
	if (!ft->seekable)
	       warn("Length in output .DVMS header will wrong since can't seek to fix it");
	p->swapbits = 0;
}

/* ---------------------------------------------------------------------- */

void
dvmsstopwrite(ft)
ft_t ft;
{
	struct dvms_header hdr;
	
	cvsdstopwrite(ft);
	if (!ft->seekable)
		return;
	if (fseek(ft->fp, 0L, 0) != 0)
		fail("Can't rewind output file to rewrite DVMS header.");
	make_dvms_hdr(ft, &hdr);
	dvms_write_header(ft->fp, &hdr);
}

/* ---------------------------------------------------------------------- */