/*---------------------------------------------------------------------------*\

	FILE....: ADPCM.CPP
	TYPE....: C++ Module
	AUTHOR..: Horse
	DATE....: 19/5/98

	ADPCM Functions for VLC card.

\*--------------------------------------------------------------------------*/

/*---------------------------------------------------------------------------*\

         Voicetronix Voice Processing Board (VPB) Software

         Copyright (C) 1999-2001 Voicetronix www.voicetronix.com.au

         This library is free software; you can redistribute it and/or
         modify it under the terms of the GNU Lesser General Public
         License as published by the Free Software Foundation; either
         version 2.1 of the License, or (at your option) any later version.

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

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

\*---------------------------------------------------------------------------*/

#include "adpcm.h"
#include "wobbly.h"
#include <assert.h>
#include <stdlib.h>

#define CODES	4			// number of codes in each word		
#define BITS	4			// number of bits per code			
#define WORD	16			// number of bits in word			
#define ADMAX	2047		// maximum ADPCM sample value		
#define ADPCM_SCALE     4	// scales linear to ADPCM 			

typedef struct {
    short pX_;				// previous decoded sample			
    int   index;			// current index					
} ADPCM;


// look up tables for Dialogic ADPCM algorithm.

static short table1[] = {-1,-1,-1,-1, 2, 4, 6, 8};
static short table2[] = {
  16,  17,  19,  21,  23,  25,  28,  31,  34,  37,  41,  45,
  50,  55,  60,  66,  73,  80,  88,  97, 107, 118, 130, 143,
 157, 173, 190, 209, 230, 253, 279, 307, 337, 371, 408, 449,
 494, 544, 598, 658, 724, 796, 876, 963,1060,1166,1282,1411,1522};

/*-------------------------------------------------------------------------*\
                            						    
 FUNCTION.: adpcm_pack							    
 AUTHOR...: Ian Roberts							    
 DATE.....: 12/5/98							    
									    
  Packs 4 bit ADPCM codes into an array of 16 bit words.         
                                                                           
\*-------------------------------------------------------------------------*/

void adpcm_pack(USHORT codes[], USHORT packed[], USHORT n)
//  USHORT codes[];		vector of 4n 4-bit ADPCM codes in LSB		
//  short  packed[];	vector of n packed ADPCM codes		
//  USHORT n;			length of packed[] 				
{
    int i;

    assert(codes != NULL);
    assert(packed != NULL);
    assert(n != 0);

	// note strange byte ordering, designed to give 3412 adpcm to
	// short mapping

    for(i=0; i<n; i++) {
        *packed = (*codes++)<<4;
        *packed += (*codes++);
        *packed += (*codes++)<<12;
        *packed += (*codes++)<<8;

        packed++;
    }
}

/*-------------------------------------------------------------------------*\
                            						    
 FUNCTION.: adpcm_unpack							    
 AUTHOR...: H Roberts							    
 DATE.....: 12/5/98							    
									    
 SYNOPSIS.: Unpacks 4 bit ADPCM codes from an array of 16 bit words.       
									    
\*-------------------------------------------------------------------------*/

void adpcm_unpack(USHORT codes[], USHORT packed[], USHORT n)
//  USHORT codes[];		vector of 4n 4-bit ADPCM codes in LSB		
//  short  packed[];	vector of n packed ADPCM codes		
//  USHORT n;			length of packed 				
{
    int i;

    assert(codes != NULL);
    assert(packed != NULL);
    assert(n != 0);

	// note strange byte ordering, designed to give 3412 adpcm to
	// short mapping

    for(i=0; i<n; i++) {
        *codes++ = (*packed >> 4) & 0xf;
        *codes++ = (*packed) & 0xf;
        *codes++ = (*packed >> 12) & 0xf;
        *codes++ = (*packed >> 8) & 0xf;
        packed++;
    }
}

/*--------------------------------------------------------------------------*\

	FUNCTION.: adpcm_open
	AUTHOR...: David Rowe
	DATE.....: 15/10/97

	Creates and initialises the ADPCM state variables.  This function
	should be called at the start of each recording or playback, the
	comp_adpcm_close should be called at the end to free allocated
	memory.

	Note that the encode and decoder require separate state variables.

\*--------------------------------------------------------------------------*/

void adpcm_open(void **states)
//  void   **states;	ADPCM encoder or decoder states		
{
    ADPCM  *adpcm;	/* ptr to ADPCM state variables structure	*/

	assert(states != NULL);

    *states = malloc(sizeof(ADPCM));
    if (*states == NULL) 
		throw Wobbly(ADPCM_OUT_OF_MEMORY);

    adpcm = (ADPCM*)*states;
    adpcm->pX_ = 0;
    adpcm->index = 0;
}

/*--------------------------------------------------------------------------*\

	FUNCTION.: adpcm_reset
	AUTHOR...: David Rowe
	DATE.....: 7/8/01

	Re-initialises the ADPCM state variables.

\*--------------------------------------------------------------------------*/

void adpcm_reset(void *states)
//  void   **states;	ADPCM encoder or decoder states		
{
    ADPCM  *adpcm;	/* ptr to ADPCM state variables structure	*/

    assert(states != NULL);
    adpcm = (ADPCM*)states;

    adpcm->pX_ = 0;
    adpcm->index = 0;
}

/*--------------------------------------------------------------------------*\

	FUNCTION.: adpcm_close
	AUTHOR...: David Rowe
	DATE.....: 15/10/97

	Frees memory allocated to ADPCM state variables.  Call at the end of
	a record/playback session.

\*--------------------------------------------------------------------------*/

void adpcm_close(void *states)
//  void   *states;		ADPCM encoder or decoder states		
{
    assert(states != NULL);
    free(states);
}

 /*--------------------------------------------------------------------------*\

	FUNCTION.: adpcm_encode
	AUTHOR...: David Rowe
	DATE.....: 30/6/94

	Encodes a frame of linear samples to a frame of 4 bit APDCM codes
	using the Dialogic ADPCM algorithm.  This version has been modified
	to use input samples in Q15 format.

	Note that the encode and decoder require separate state variables.

\*--------------------------------------------------------------------------*/

void adpcm_encode(void *states, USHORT codes[], short X[], USHORT n)
//  void   *states;	ADPCM encoder states				
//  USHORT codes[];	vector of n 4 bit ADPCM codes in LSB		
//  short  X[];		vector of n linear input samples		
//  USHORT n;		length of codes[] and X[]			
{
    ADPCM  *adpcm;	// ptr to ADPCM state variables structure	
    short  ss;		// ADPCM step size 				
    short  d;		// difference value to code 			
    int    code;	// 4 bit ADPCM code 				
    short  X_,pX_;	// current and previous decoded ADPCM sample	
    int    index;	// current table index				
    int i;

    /* argument validation */

    assert(states != NULL);
    assert(codes != NULL);
    assert(X != NULL);
    assert(n != 0);

    /* Initialise */

    adpcm = (ADPCM*)states;
    pX_ = adpcm->pX_;
    index = adpcm->index;
    ss = table2[index];

    for(i=0; i<n; i++) {

	/* ADPCM encoder */

	code = 0;
	d = (X[i]>>ADPCM_SCALE) - pX_;
	if (d < 0.0)
	    code = 8;
	d = abs(d);
	if (d >= ss) {
	    d = d - ss;
	    code += 4;
	}

	if (d >= ss/2) {
	    d = d - ss/2;
	    code += 2;
	}

	if (d >= ss/4)
	    code += 1;

	codes[i] = code;

	/* ADPCM decoder */

	/*#define FLOAT*/
	#ifdef FLOAT
	X_ = (code & 0x7)*(ss/4) +(ss/8);
	#else
	X_ = (code & 0x7)*(ss>>2) +(ss>>3);
	#endif

	if (code & 0x8)
	    X_ = -X_;
	X_ += pX_;
	pX_ = X_;

	/* Step size calculation */

	index += table1[code & 0x7];
	if (index < 0) index = 0;
	if (index > 48) index = 48;
	ss = table2[index];
    }

    /* store state variables for next iteration */

    adpcm->pX_ = pX_;
    adpcm->index = index;

}

/*--------------------------------------------------------------------------*\

	FUNCTION.: adpcm_decode
	AUTHOR...: David Rowe
	DATE.....: 30/6/94

	Decodes a frame of 4 bit APDCM codes to a frame of speech samples
	using the Dialogic ADPCM algorithm.  Also included is
	a hard limiter on the output samples that tends to remove any DC
	offset.  DC offsets can be introduced if the encoder and decoder
	state variables are not reset at the same time, for example at the
	start of a logged message.  However, it is still reccomended that
	the state variable of both the encoder and decoder be initialised
	at the start of each recording.

	Note that the encode and decoder require separate state variables.

	The ouput samples are in Q15 format.

\*--------------------------------------------------------------------------*/

void adpcm_decode(void *states, short X_[], USHORT codes[], USHORT n)
//  void   *states;		ADPCM decoder states				
//  short  X_[];		vector of n linear output samples		
//  USHORT codes[];		vector of n 4 bit ADPCM codes in LSB		
//  USHORT n;			length of codes[] and X[]			
{
    ADPCM  *adpcm;	// ptr to ADPCM state variables structure	
    short  pX_;		// current and previous decoded ADPCM sample	
    int    index;	// current table index			
    short  ss;		// ADPCM step size 				
    int    code;	// 4 bit ADPCM code 			
    int    i;		// loop variables 				

    /* argument validation */

    assert(states != NULL);
    assert(codes != NULL);
    assert(X_ != NULL);
    assert(n != 0);

    /* Initialise */

    adpcm = (ADPCM*)states;
    pX_ = adpcm->pX_;
    index = adpcm->index;
    ss = table2[index];

    for(i=0; i<n; i++) {

	    code = codes[i];
	    assert((code & ~0xf) == 0);	/* should only use 4 LSB */

	    /* ADPCM decoder */

	    /*#define FLOAT*/
	    #ifdef FLOAT
	    X_[i] = (code & 0x7)*(ss/4) +(ss/8);
	    #else
	    X_[i] = (code & 0x7)*(ss>>2) +(ss>>3);
	    #endif

	    if (code & 0x8)
	        X_[i] = -X_[i];
	    X_[i] += pX_;

	    /* Self centering algorithm copied from Electrodata code */

	    if (X_[i] > ADMAX) X_[i] = ADMAX;
	    if (X_[i] < -ADMAX) X_[i] = -ADMAX;

	    pX_ = X_[i];
	    X_[i] <<= ADPCM_SCALE;

	    /* Step size calculation */

	    index += table1[code & 0x7];
	    if (index < 0) index = 0;
	    if (index > 48) index = 48;
	    ss = table2[index];
    }

    /* store state variables for next iteration */

    adpcm->pX_ = pX_;
    adpcm->index = index;

}

