#include <string.h>
#include <stdlib.h>
#include "splear.h"
#include "iap.h"
#include "lg2.h"
#include "pwm.h"
#include "pdmspi.h"
#include "mulaw.h"


/* Systick timer tick rate, to change duty cycle */
#define TICKRATE_HZ     1000		/* 1 ms Tick rate */

/*
 * SPL parameters

		//ssq += (pcm-avg)*(pcm-avg);
		sabs += pcm > 0 ? pcm : -pcm;

		// Finish SPL and apply it at each complete SPL window interval
		++n;
		if (monitor & PRINT_SAMPLES) {
			extern void putLineUARTn(const uint8_t *, size_t);
			//int s =  (pcm + 8192) >> 6;
			int s =  linear14_ulaw(pcm);
			//s = n & 0xff;

			sbuf[n & 63] = s <= 0 ? 0 : s >=255 ? 255 : s;
			if ((n&31) == 31)
				putLineUARTn(sbuf + (n & 32), 32);
		}
		if (n == WINDOWSIZE) {

/*
 * mulaw.c
 *
 *  Created on: Mar 13, 2015
 *      Author: Ross
 *
 * Good source for g711.c seems to be from pulseaudio sources where
 * they are deliberately starting with 14-bit samples. The referenced
 * page also has a complete lookup table set for doing both a-Law and
 * µ-Law to and from linear coding with lookup tables. 256 entries for
 * the decoder. 8K entries for aLaw encoder and 16K entries for µ-Law
 * encoder, so we won't be implementing the encoder with tables.
 *
 * https://fossies.org/linux/pulseaudio/src/pulsecore/g711.c
 */
#include <stdint.h>

#if (!defined(USE_TABLE_LOOKUP) || !defined(USE_CLZ_EMULATED))
#define USE_TABLE_LOOKUP
//#define USE_CLZ_EMULATED
#endif


#ifdef USE_TABLE_LOOKUP
/*
 * Table of thresholds for implementing count of leading zeros for computing
 * the segment number from the biased 14 bit sample value.
 */
static int16_t seg_uend[8] = {0x3F, 0x7F, 0xFF, 0x1FF, 0x3FF, 0x7FF, 0xFFF, 0x1FFF};
static int16_t search(
		      int16_t        val,
		      int16_t *table,
		      int size)
{
    int i;

    for (i = 0; i < size; i++) {
	if (val <= *table++)
	    return (i);
    }
    return (size);
}
static unsigned int segment14(unsigned short x)
{
	return search(x, seg_uend, 8);
}

#else
#ifdef USE_CLZ_EMULATED
#if 0
static unsigned int clz32(unsigned int x)
{
	int n = 0;

    if (x = 0)
    	return 32;
    if (!(x & 0xFFFF0000)) { n += 16; x <<= 16; }
    if (!(x & 0xFF000000)) { n += 8; x <<= 8; }
    if (!(x & 0xF0000000)) { n += 4; x <<= 4; }
    if (!(x & 0xC0000000)) { n += 2; x <<= 2; }
    if (!(x & 0x80000000)) { n += 1; /*x <<= 1;*/ }
    return n;
}
#endif
static unsigned int clz16(unsigned short x)
{
	int n = 0;

    if (x = 0)
    	return 16;
    if (!(x & 0xFF00)) { n += 8; x <<= 8; }
    if (!(x & 0xF000)) { n += 4; x <<= 4; }
    if (!(x & 0xC000)) { n += 2; x <<= 2; }
    if (!(x & 0x8000)) { n += 1; /*x <<= 1;*/ }
    return n;
}
static unsigned int segment14(unsigned short x)
{
	int n = clz16(x);
	return n >= 10 ? 0 : 10-n;
#if 0
	int n = 0;

    //if (x = 0)    	return 0;
    if (!(x & 0xFE00)) { n += 4; x <<= 4; }
    if (!(x & 0xF800)) { n += 2; x <<= 2; }
    if (!(x & 0xF000)) { n += 1; /*x <<= 1;*/ }
    return 7-n;
#endif
}
#endif
#endif

#define        BIAS                (0x84)                /* Bias for linear code. */
#define CLIP            8159

/*
 * linear2ulaw() accepts a 14-bit signed integer and encodes it as u-law data
 * stored in a unsigned char.  This function should only be called with
 * the data shifted such that it only contains information in the lower
 * 14-bits.
 *
 * In order to simplify the encoding process, the original linear magnitude
 * is biased by adding 33 which shifts the encoding range from (0 - 8158) to
 * (33 - 8191). The result can be seen in the following encoding table:
 *
 *        Biased Linear Input Code        Compressed Code
 *        ------------------------        ---------------
 *        0 0000 001w xyza                        000wxyz
 *        0 0000 01wx yzab                        001wxyz
 *        0 0000 1wxy zabc                        010wxyz
 *        0 0001 wxyz abcd                        011wxyz
 *
 *        0 001w xyza bcde                        100wxyz
 *        0 01wx yzab cdef                        101wxyz
 *
 *        0 1wxy zabc defg                        110wxyz
 *        1 wxyz abcd efgh                        111wxyz
 *
 * Each biased linear code has a leading 1 which identifies the segment
 * number. The value of the segment number is equal to 7 minus the number
 * of leading 0's. The quantization interval is directly available as the
 * four bits wxyz.  * The trailing bits (a - h) are ignored.
 *
 * Ordinarily the complement of the resulting code word is used for
 * transmission, and so the code word is complemented before it is returned.
 *
 * For further information see John C. Bellamy's Digital Telephony, 1982,
 * John Wiley & Sons, pps 98-111 and 472-476.
 */
unsigned char linear14_ulaw(
			       int16_t                pcm_val)        /* 2's complement (14-bit range) */
{
    int16_t                mask;
    int16_t                seg;
    unsigned char        uval;

	/* Have calling software do it since its already doing a shift
         * from 32-bits down to 16-bits.
         */
	/* pcm_val = pcm_val >> 2; */

	/* u-law inverts all bits */
	/* Get the sign and the magnitude of the value. */
    if (pcm_val < 0) {
	pcm_val = -pcm_val;
	mask = 0x7F;
    } else {
	mask = 0xFF;
    }
    if ( pcm_val > CLIP ) pcm_val = CLIP;                /* clip the magnitude */
    pcm_val += (BIAS >> 2);

	/* Convert the scaled magnitude to segment number. */
    seg = segment14(pcm_val);
    //seg = search(pcm_val, seg_uend, 8);

	/*
         * Combine the sign, segment, quantization bits;
         * and complement the code word.
         */
    if (seg >= 8)                /* out of range, return maximum value. */
	return (unsigned char) (0x7F ^ mask);
    else {
	uval = (unsigned char) (seg << 4) | ((pcm_val >> (seg + 1)) & 0xF);
	return (uval ^ mask);
    }

}

#ifdef TEST_CODE
#include <stdio.h>

int table()
{
	int x, y;
	printf("uint8_t _st_14linear2ulaw[0x4000] = {\n  ");
	y = 0;
	for (x = 0; x < 0x4000; x++)
	{
		printf(" 0x%02x,", linear14_ulaw((-0x2000)+x));
		y++;
		if (y == 12)
		{
			y = 0;
			printf("\n  ");
		}
	}
	printf("\n};\n");

}

int sample(void)
{
	 static unsigned int b[8] = {0x3F, 0x7F, 0xFF, 0x1FF, 0x3FF, 0x7FF, 0xFFF, 0x1FFF};
	 int i;

	 for (i=0; i<8; ++i) {
		 printf("%4x  %02x    %4x %02x\n", b[i], linear14_ulaw(b[i]),  0x3fff & ((-0x2000)+b[i]), linear14_ulaw(b[i]-0x2000));
		 printf("%4x  %02x    %4x %02x\n", b[i]+1, linear14_ulaw(b[i]+1), 0x3fff & (b[i]+1-0x2000), linear14_ulaw(b[i]+1-0x2000));
	 }
}

int main(int argc, char **argv)
{
//	sample();
	table();
}
#endif

/*
 * mulaw.h
 *
 *  Created on: Mar 16, 2015
 *      Author: Ross
 */

#ifndef MULAW_H_
#define MULAW_H_
#include <stdint.h>

unsigned char linear14_ulaw(int16_t pcm_val);        /* 2's complement (14-bit range) */

#endif /* MULAW_H_ */