/********************************************************************
 *                                                                  *
 * THIS FILE IS PART OF THE OggVorbis SOFTWARE CODEC SOURCE CODE.   *
 * USE, DISTRIBUTION AND REPRODUCTION OF THIS LIBRARY SOURCE IS     *
 * GOVERNED BY A BSD-STYLE SOURCE LICENSE INCLUDED WITH THIS SOURCE *
 * IN 'COPYING'. PLEASE READ THESE TERMS BEFORE DISTRIBUTING.       *
 *                                                                  *
 * THE OggVorbis SOURCE CODE IS (C) COPYRIGHT 1994-2009             *
 * by the Xiph.Org Foundation http://www.xiph.org/                  *
 *                                                                  *
 ********************************************************************

  function: LSP (also called LSF) conversion routines
  last mod: $Id: lsp.c 16227 2009-07-08 06:58:46Z xiphmont $

  The LSP generation code is taken (with minimal modification and a
  few bugfixes) from "On the Computation of the LSP Frequencies" by
  Joseph Rothweiler (see http://www.rothweiler.us for contact info).
  The paper is available at:

  http://www.myown1.com/joe/lsf

 ********************************************************************/

/* Note that the lpc-lsp conversion finds the roots of polynomial with
   an iterative root polisher (CACM algorithm 283).  It *is* possible
   to confuse this algorithm into not converging; that should only
   happen with absurdly closely spaced roots (very sharp peaks in the
   LPC f response) which in turn should be impossible in our use of
   the code.  If this *does* happen anyway, it's a bug in the floor
   finder; find the cause of the confusion (probably a single bin
   spike or accidental near-float-limit resolution problems) and
   correct it. */

#include <stdlib.h>
#include "lsp.h"
#include "os.h"

/* three possible LSP to f curve functions; the exact computation
   (float), a lookup based float implementation, and an integer
   implementation.  The float lookup is likely the optimal choice on
   any machine with an FPU.  The integer implementation is *not* fixed
   point (due to the need for a large dynamic range and thus a
   seperately tracked exponent) and thus much more complex than the
   relatively simple float implementations. It's mostly for future
   work on a fully fixed point implementation for processors like the
   ARM family. */

#include "lookup.c" /* catch this in the build system; we #include for
                       compilers (like gcc) that can't inline across
                       modules */
/*
static const int MLOOP_1[64]={
   0,10,11,11, 12,12,12,12, 13,13,13,13, 13,13,13,13,
  14,14,14,14, 14,14,14,14, 14,14,14,14, 14,14,14,14,
  15,15,15,15, 15,15,15,15, 15,15,15,15, 15,15,15,15,
  15,15,15,15, 15,15,15,15, 15,15,15,15, 15,15,15,15,
};

static const int MLOOP_2[64]={
  0,4,5,5, 6,6,6,6, 7,7,7,7, 7,7,7,7,
  8,8,8,8, 8,8,8,8, 8,8,8,8, 8,8,8,8,
  9,9,9,9, 9,9,9,9, 9,9,9,9, 9,9,9,9,
  9,9,9,9, 9,9,9,9, 9,9,9,9, 9,9,9,9,
};

static const int MLOOP_3[8]={0,1,2,2,3,3,3,3};
*/
static const int32_t PI_CL = 20861; // (xint)(double)((1<<16)/M_PI);

static int shift16(uint32_t i)
{
//			if(!(shift=MLOOP_1[(pi|qi)>>25]))
//				if(!(shift=MLOOP_2[(pi|qi)>>19]))
//					shift=MLOOP_3[(pi|qi)>>16];
	int ret;

	if(i>>23)
	{
		ret = 8;
		i >>= 8;
	}
	else	ret = 0;
	if(i>>19)
	{
		ret += 4;
		i >>= 4;
	}
	if(i>>17)
	{
		ret += 2;
		i >>= 2;
	}
	if(i>>16)
		ret += 1;

	return ret;
}

/* side effect: changes *lsp to cosines of lsp */
void vorbis_lsp_to_curve(xint* curve,int* map,int n,bint* lsp,int m,
			    bint amp, int32_t ampoffset)
{
	/* 0 <= m < 256 */

	/* set up for using all int later */
	int i, jmax;
	int32_t ampoffseti = ampoffset<<12;
	int32_t ampi = amp>>(BISHIFT-4);
	bint* plsp = lsp;

	for(i = m; i ; i--, plsp++)
		*plsp = vorbis_coslook_i(LMULBI(*plsp,PI_CL));

	i = 0;
	jmax = (m>>1) - 1;
	while(i < n)
	{
		int32_t wi = map[i]; // in 0.14 (vorbis_coslook_i was done in floor)
		int qexp = 0, shift;
		int32_t d;
		uint32_t pi; // in 0.16
		uint32_t qi;
		int j;

		plsp = lsp;

		d = plsp[0]-wi;
		if (d < 0) d = -d;
		qi = d << 16;
		d = plsp[1]-wi;
		if (d < 0) d = -d;
		pi = d << 16;
		shift = 14;

		for(j = jmax; j; j--)
		{
			plsp += 2;
			d = plsp[0]-wi;
			if (d < 0) d = -d;
			qi = (qi>>shift)*d;
			d = plsp[1]-wi;
			if (d < 0) d = -d;
			pi = (pi>>shift)*d;
			qexp += shift;
			shift = shift16(pi|qi);
		}

		/* pi,qi normalized collectively, both tracked using qexp */

		if(m & 1)
		{
			// odd order filter; slightly assymetric
			// the last coefficient
			d = plsp[2]-wi;
			if (d < 0) d = -d;
			qi = (qi>>shift)*d;
			pi = (pi>>shift)<<14;
			qexp += shift;

			shift = shift16(pi|qi);

			pi >>= shift;
			qi >>= shift;
			qexp += shift-14*((m+1)>>1);

			pi = ((pi*pi)>>16);
			qi = ((qi*qi)>>16);
			qexp = qexp*2+m-1;

			pi*=(1<<14)-((wi*wi)>>14);
			qi+=pi>>14;
		}
		else
		{
			// even order filter; still symmetric

			// p*=p(1-w), q*=q(1+w), let normalization drift
			// because it isn't worth tracking step by step

			pi>>=shift;
			qi>>=shift;
			qexp+=shift-7*m;

			pi=((pi*pi)>>16);
			qi=((qi*qi)>>16);
			qexp=qexp*2+m-1;

			pi*=(1<<14)-wi;
			qi*=(1<<14)+wi;
			qi=(qi+pi)>>14;
		}

		// we've let the normalization drift because it wasn't important
		// however, for the lookup, things must be normalized again.  We
		// need at most one right shift or a number of left shifts

		if (qi)
		{
			if (qi & 0xffff0000)
			{ // checks for 1.xxxxxxxxxxxxxxxx
				qi >>= 1;
				qexp++;
			}
			else
			{
				if (!(qi & 0xff00))
				{
					qi<<=8;
					qexp-=8;
				}
				if (!(qi & 0xf000))
				{
					qi<<=4;
					qexp-=4;
				}
				if (!(qi & 0xc000))
				{
					qi<<=2;
					qexp-=2;
				}
				if (!(qi & 0x8000))
				{
					qi<<=1;
					qexp-=1;
				}
/*				while(!(qi&0x8000))
				{ // checks for 0.0xxxxxxxxxxxxxxx or less
					qi<<=1;
					qexp--;
		        	}
*/		        }
		}

		{
			int32_t shift;
			int32_t iamp=vorbis_fromdBlook_i(ampi*	//  n.4
				    vorbis_invsqlook_i(qi,qexp)-
							// m.8, m+n<=8
				    ampoffseti	// 8.12[0]
                                   , &shift);
            shift -= XISHIFT - BISHIFT;
			curve[i] = LMULSHR(curve[i], iamp, shift);
			while(map[++i] == wi) // no risk, map[n] has special val
				curve[i] = LMULSHR(curve[i], iamp, shift);
		}
	}
}
