/*************************************************************************\
  rs16.c:
    16bit version of "resample.c". Input and output format can be any
    combination of mono/stereo intel/motorola/byte samples, but only
    raw data.
\*************************************************************************/

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <ctype.h>
#include <limits.h>
#include <signal.h>
#include <time.h>
#include <math.h>
#include "remez.h"

char *version = "$VER: rs16 1.1 (04.12.99)";


#define IOBUFSIZE 16384      /* we'll tell C to use bigger file buffers */

enum { FALSE = 0, TRUE };
enum { FMT_BYTE = 0, FMT_MOT, FMT_INTEL, FMT_NONE };

void CreateFilter( void );
long Resample( long len );

FILE *infile, *outfile;
float fstop, stopwt = 5;  /* filter parameters */
float fpass = 0.7;
short a[ NFMAX ];   /* filter coefficients, fixed point representation */
int shift;          /* denotes fixed point unit */
int n = 0;
int intpl = 0, decmt = 0;
float gain = 1;
int verbose = FALSE;
int ifmt = FMT_MOT, ofmt = FMT_NONE;
int ichan = 2, ochan = 0;


void help( char *s )
    {
    if( s )
        printf( "Illegal parameter '%s'!\n", s );
    printf( "16-bit resample. Usage: rs16 <infile> [<outfile>] [options]\n" );
    printf( "Where <outfile> defaults to <infile>.out and options are:\n" );
    printf( "  -s/-S<input/output samples> (m[otorola]/i[ntel]/b[yte])\n" );
    printf( "  -c/-C<input/output channels> (1/2)\n" );
    printf( "  -g<gain> (i.e. amplification)\n" );
    printf( "  -i<interpolation ratio>\n" );
    printf( "  -d<decimation ratio>\n" );
    printf( "  -l<filter length>\n" );
    printf( "  -p<pass band width>\n" );
    printf( "  -w<weight of stop band>\n" );
    printf( "  -v[erbose]\n" );
    exit( s ? 10 : 0 );
    }

int main( int argc, char* argv[] )
    {
    long rawsize = 0;
    int caps;
    char *s;
    char name1[ 200 ], name2[ 200 ];
    char *fmts = "bmi";
    char *fmtname[] = { "8-bit", "motorola", "intel" };
    char *chname[] = { "", "mono", "stereo" };

    if( argc < 2 )
        help( NULL );
    name1[ 0 ] = name2[ 0 ] = '\0';
    while( --argc )
        {
        s = *++argv;
        if( *s != '-')
            {
            if( name1[0] == '\0' )
                strcpy( name1, s );
            else
                strcpy( name2, s );
            }
        else
            {
            s++;
            caps = isupper( *s );
            switch( *s++ )
                {
                case 'l':
                    n = atoi( s );
                    break;
                case 'd':
                    decmt = atoi( s );
                    break;
                case 'i':
                    intpl = atoi( s );
                    break;
                case 'g':
                    gain = atof( s );
                    break;
                case 'p':
                    fpass = atof( s );
                    break;
                case 'w':
                    stopwt = atof( s );
                    break;
                case 'v':
                    verbose = 1;
                    break;
                case 's':
                case 'S':
                    if( *s == '\0' || strchr( fmts, *s ) == NULL )
                        help( argv[ 0 ] );
                    if( caps )
                        ofmt = strchr( fmts, *s ) - fmts;
                    else
                        ifmt = strchr( fmts, *s ) - fmts;
                    break;
                case 'c':
                case 'C':
                    if( *s != '1' && *s != '2' )
                        help( argv[ 0 ] );
                    if( caps )
                        ochan = *s - '0';
                    else
                        ichan = *s - '0';
                    break;
                default:
                    help( argv[ 0 ] );
                }
            }
        }
    /* add some default values: */
    if( intpl < 1 )
        intpl = 1;
    if( decmt < 1 )
        decmt = (intpl == 1) ? 2 : 1;
    if( n < 1 )
        n = (decmt > intpl) ? 10*decmt+1 : 10*intpl+1;
    if( ofmt == FMT_NONE )
        ofmt = ifmt;
    if( ochan == 0 )
        ochan = ichan;
    /* make sure the arrays don't overflow */
    if( n > NFMAX )
        n = NFMAX;
    if( name1[0] == '\0' )
        {
        printf("Please specify an input file!\n");
        return 5;
        }
    if (name2[0] == '\0')
        {
        strcpy(name2, name1);
        strcat(name2, ".out");
        }
    if( !(infile = fopen(name1,"rb")) )
        {
        printf("Can't open %s!\n", name1);
        return 10;
        }
    setvbuf( infile, NULL, _IOFBF, IOBUFSIZE );
    fseek( infile, 0L, SEEK_END );
    rawsize = ftell( infile );  /* size in bytes */
    if( ichan > 1 )
        rawsize /= 2;
    if( ifmt != FMT_BYTE )
        rawsize /= 2;           /* -> size in samples */
    rewind( infile );
    if( !(outfile = fopen(name2,"wb")) )
        {
        printf("Can't open %s for output!\n", name2);
        fclose(infile);
        return 10;
        }
    setvbuf( outfile, NULL, _IOFBF, IOBUFSIZE );
    /* tell the user what his parameters will do */
    printf( "Resampling \"%s\" as \"%s\",\n", name1, name2);
    printf( "input samples: %s %s, output samples: %s %s,\n",
        fmtname[ ifmt ], chname[ ichan ], fmtname[ ofmt ], chname[ ochan ] );
    printf( "  using a %d-point FIR filter (-l), gain %.2f (-g),\n", n, gain);
    printf( "  interpolation/decimation ratio is %d:%d (-i, -d),\n", intpl, decmt);
    printf( "  pass band ends at %.2f below the stop band (-p),\n", fpass);
    printf( "  relative weight of stop band errors is %.1f (-w).\n", stopwt);
    CreateFilter();
    Resample( rawsize );
    fclose( infile );
    fclose( outfile );

    return 0;
    }



/* Create filter coefficients a[0..n-1], normalized, so that sum(a[i])==1,
 * and converted to a fixed point representation.
 */
void CreateFilter( void )
    {
    float sum=0, max=0;
    long isum, unit;
    int k;

    /* filter parameters: */
    fstop = (decmt > intpl) ? 0.5/decmt : 0.5/intpl;
    fpass *= fstop;   /* e. g. fpass = 0.7 * fstop */
    printf( "Calculating filter parameters" );
    fflush(stdout);
    remez_dot = 1;    /* will create "....." output */
    if( makefilter( n ) > 0 )   /* routine from "remez.o" */
        {
        printf(" convergence failure, sorry.\n");
        if( n%2 == 0 )
            printf("Better try '-l%d'.\n", n+1);
        exit( 10 );
        }
    /* Convert the impulse response from float h[] to short a[], */
    /* but make sure it's normalized. ("Normalized": an input stream */
    /* of all 1's would come out as all 1's again.) */
    for( k=0; k<n; k++ )
        {
        sum += h[ k ];
        if( max < fabs( h[ k ] ) )
            max = fabs( h[ k ] );
        }
    /* find the maximum possible fixed point unit */
    shift = 0;
    do  {
        shift++;
        unit = 1 << shift;
    } while( unit*max * intpl*gain/sum < 0x8000 && unit < 0x10000 );
    shift--;
    unit = 1 << shift;
    isum = 0;
    for( k=0; k<n; k++ )
        {
        a[k] = unit * h[k] * intpl*gain/sum;
        isum += a[k];
        }
    printf( " done, unit: 0x%lx, transmit: 0x%lx\n", unit, isum );
    if( verbose )
        for( k=0; k<n; k++ )
            printf( "h[%2d] = %8.5f  (%6d)\n", k, h[k], a[k] );
    printf( "Pass band [0:%.2f], ripple ± %.1f %%.\n", fpass,
        100*fabs(extreme(0)-1));
    printf( "Stop band [%.2f:0.5], attenuation %.0f dB.\n", fstop,
        20*log10(fabs(extreme(0.5))) );
    }



float desire( float freq )
    {
    if( freq > fstop )
        return 0;
    else
        return 1;
    }

float weight(float freq)
    {
    if (freq > fstop)
        return stopwt;
    else if (freq > fpass)
        return 0;
    else
        return 1;
    }



int ctrl_c = 0;

void oops()
/* break-handler during filter operation */
    {
    ctrl_c = 1;
    }



/* Read one left and right 16bit sample from the input file.
 * Works correctly for 8bit input files, too.
 * If the input file is mono, both values will be the same.
 */
void GetSample( short *pleft, short *pright )
    {
    int i, c = 0;

    for( i = 0; i < ichan; i++ )
        {
        switch( ifmt )
            {
            case FMT_BYTE:
                c = fgetc( infile ) << 8;
                break;
            case FMT_MOT:
                c = fgetc( infile ) << 8;
                c |= (unsigned char)fgetc( infile );
                break;
            case FMT_INTEL:
                c = fgetc( infile );
                c |= fgetc( infile ) << 8;
                break;
            }
        switch( i )
            {
            case 0:
                *pleft = c;
                /* drop through! */
            case 1:
                *pright = c;
                break;
            }
        }
    }



/* Write a 16bit left/right sample pair to the output file.
 * Works correctly for 8bit output files, too.
 * If the output file is mono, only the left value will be used
 * (no averaging!).
 */
void PutSample( short left, short right )
    {
    int i;
    unsigned c;

    c = left;
    for( i = 0; i < ochan; i++ )
        {
        switch( ofmt )
            {
            case FMT_BYTE:
                fputc( c >> 8, outfile );
                break;
            case FMT_MOT:
                fputc( c >> 8, outfile );
                fputc( c & 0xff, outfile );
                break;
            case FMT_INTEL:
                fputc( c & 0xff, outfile );
                fputc( c >> 8, outfile );
                break;
            }
        c = right;
        }
    }



/* Do the filtering. Parameter and return value are sample counts,
 * which may correspond to 1, 2 or 4 bytes each.
 * The return value may be more or less than the original value,
 * depending on what interpolation/decimation ratio is applied.
 */
long Resample( long len )
    {
    short bl[ NFMAX ], br[ NFMAX ];     /* input signal buffers */
    short ol = 0, or = 0;               /* output samples */
    long max, loop, outlen = 0;
    long sum, peak = 0;
    long warnct = 0, warnbase = 0;
    int i, j, k, ch, channels = 2;
    int modi = 1, modd = 0;     /* modulo counters */
    char modbyte = 0;
    time_t t0;

    signal( SIGINT, oops );     /* intercept user break (Ctrl-C) */
    t0 = time( NULL );
    if( ichan < 2 || ochan < 2 )
        channels = 1;
    for( i = 0; i < n; i++ )
        {
        bl[ i ] = 0;
        br[ i ] = 0;
        }
    i = 0;
    max = len*intpl + n / 2;
    for( loop = 0; loop < max; loop++ )
        {
        bl[ i ] = 0;
        br[ i ] = 0;
        if( --modi == 0 )
            {                   /* read one sample */
            modi = intpl;
            if( len-- > 0 )
                {
                GetSample( &bl[ i ], &br[ i ] );
                if( ichan > channels )  /* make (left) mono by averaging */
                    bl[ i ] = ((long)bl[ i ] + (long)br[ i ]) / 2;
                }
            }
        i++;
        if( i == n )
            i = 0;              /* i points to the oldest value */
        if( modd <= 0 && loop == n/2 )
            modd = 1;           /* enable output */
        if( --modd == 0 )
            {                   /* write one sample */
            modd = decmt;
            /* filter operation, maybe two channels */
            for( ch = 0; ch < channels; ch++ )
                {
                sum = 0;
                k = i;
                for( j = 0; j < n; j++ )
                    {
                    if( ch == 0 )
                        sum += a[ j ] * bl[ k ];
                    else
                        sum += a[ j ] * br[ k ];
                    k++;
                    if( k == n )
                        k = 0;
                    }
                sum >>= shift;
                or = sum;
                if( sum > SHRT_MAX )
                    or = SHRT_MAX;
                if( sum < SHRT_MIN )
                    or = SHRT_MIN;
                /* split left/right channel */
                if( ch == 0 )
                    ol = or;
                /* overshoot statistics */
                warnbase++;
                if( or != sum )
                    warnct++;
                if( peak < sum )
                    peak = sum;
                if( peak < -sum )
                    peak = -sum;
                }
            PutSample( ol, or );
            outlen++;
            }
        /* progress indicator, once every second */
        if( len == 0 || (modbyte++ == 0 && t0 != time( NULL )) )
            {
            float ratio, amount;

            t0 = time( NULL );
            printf( "\r%ld input samples to go, ", len );
            ratio = warnbase ? warnct * 100.0 / warnbase : 0;
            printf( "overshoot ratio %.1f %%", ratio );
            amount = peak * 100.0 / SHRT_MAX;
            printf( " (peak at %.0f %%)", amount );
            printf( "\e[K" );   /* that's ClrEoL */
            fflush( stdout );
            if( ratio > 2 )
                printf( "\n" );
            warnct = 0;
            warnbase = 0;
            peak = 0;
            }
        if( ctrl_c )
            {
            printf("\n*** BREAK - output sample is incomplete!");
            break;
            }
        }
    printf("\n");
    signal(SIGINT, SIG_DFL);  /* restore normal handling */

    return outlen;
    }