path: root/tests/test5/compute.c

#include "compute.h"

#include <stdlib.h>
#include <math.h>

#define MaxFastBits 16

int **gFFTBitTable = NULL;

gfloat compute_level(const float *data, size_t nsamples, size_t nchan) {

	size_t i; 
	float level=0;
	float *input = malloc(nsamples*sizeof(float));
	float *output = malloc(nsamples*sizeof(float));

	double rate=44100; //TODO dynamique
/* Just return the max peak
	for (i=0;i<nsamples;i+=nchan) {
		val=((float *)data)[i];
		//printf("val==%i\n", val);
		if (val<0) val=-val;
		if (level<val) level=val;
	}
*/
	for (i=0;i<nsamples;i++) {
		input[i]=data[i*nchan];
	}

	compute_spectrom(input, nsamples, rate, output);

	for (i=0;i<nsamples;i++) {
		level+=output[i];
	}

	return level/nsamples;
}

// From Audacity
void compute_spectrom(float * data, int width, double rate, float *output) {

	int i;
	float processed[256]={ 0.0f };
	float in[256];
	float out[256];

	int start = 0;
	int windows = 0;
	while (start + 256 <= width) {
		for (i=0; i<256; i++)
			in[i] = data[start + i];

		// Hanning
		for (i=0; i<256; i++)
			in[i] *= 0.50 - 0.50 * cos(2 * M_PI * i / (256 - 1));
		break;

		PowerSpectrum(in, out);

		// Take real part of result
		for (i=0; i<256/2; i++)
			processed[i] += out[i];

		start += 256/2;
		windows++;
	}
	// Convert to decibels
	// But do it safely; -Inf is nobody's friend
	for (i = 0; i < 256/2; i++){
		float temp=(processed[i] / 256 / windows);
		if (temp > 0.0)
			processed[i] = 10*log10(temp);
		else
			processed[i] = 0;
	}
	for(i=0;i<256/2;i++)
		output[i] = processed[i];
}

/*
 * PowerSpectrum
 *
 * This function computes the same as RealFFT, above, but
 * adds the squares of the real and imaginary part of each
 * coefficient, extracting the power and throwing away the
 * phase.
 *
 * For speed, it does not call RealFFT, but duplicates some
 * of its code.
 */

void PowerSpectrum(float *In, float *Out)
{
	int i;

	float theta = M_PI / 128;

	float tmpReal[128];
	float tmpImag[128];
	float RealOut[128];
	float ImagOut[128];

	for (i = 0; i < 128; i++) {
		tmpReal[i] = In[2 * i];
		tmpImag[i] = In[2 * i + 1];
	}

	FFT(128, 0, tmpReal, tmpImag, RealOut, ImagOut);

	float wtemp = sin(0.5 * theta);

	float wpr = -2.0 * wtemp * wtemp;
	float wpi = -1.0 * sin(theta);
	float wr = 1.0 + wpr;
	float wi = wpi;

	int i3;

	float h1r, h1i, h2r, h2i, rt, it;
	for (i = 1; i < 128 / 2; i++) {

		i3 = 128 - i;

		h1r = 0.5 * (RealOut[i] + RealOut[i3]);
		h1i = 0.5 * (ImagOut[i] - ImagOut[i3]);
		h2r = 0.5 * (ImagOut[i] + ImagOut[i3]);
		h2i = -0.5 * (RealOut[i] - RealOut[i3]);

		rt = h1r + wr * h2r - wi * h2i;
		it = h1i + wr * h2i + wi * h2r;

		Out[i] = rt * rt + it * it;

		rt = h1r - wr * h2r + wi * h2i;
		it = -h1i + wr * h2i + wi * h2r;

		Out[i3] = rt * rt + it * it;

		wr = (wtemp = wr) * wpr - wi * wpi + wr;
		wi = wi * wpr + wtemp * wpi + wi;
	}

	rt = (h1r = RealOut[0]) + ImagOut[0];
	it = h1r - ImagOut[0];
	Out[0] = rt * rt + it * it;
	rt = RealOut[128 / 2];
	it = ImagOut[128 / 2];
	Out[128 / 2] = rt * rt + it * it;
}

void FFT(int NumSamples,
		gboolean InverseTransform,
		float *RealIn, float *ImagIn, float *RealOut, float *ImagOut)
{
	int NumBits;                 /* Number of bits needed to store indices */
	int i, j, k, n;
	int BlockSize, BlockEnd;

	double angle_numerator = 2.0 * M_PI;
	double tr, ti;                /* temp real, temp imaginary */
/*
	if (!IsPowerOfTwo(NumSamples)) {
		fprintf(stderr, "%d is not a power of two\n", NumSamples);
		exit(1);
	}
*/
	if (!gFFTBitTable)
		InitFFT();

	if (!InverseTransform)
		angle_numerator = -angle_numerator;

	NumBits = NumberOfBitsNeeded(NumSamples);

	/*
	 **   Do simultaneous data copy and bit-reversal ordering into outputs...
	 */
	for (i = 0; i < NumSamples; i++) {
		j = FastReverseBits(i, NumBits);
		RealOut[j] = RealIn[i];
		ImagOut[j] = (ImagIn == NULL) ? 0.0 : ImagIn[i];
	}

	/*
	 **   Do the FFT itself...
	 */

	BlockEnd = 1;
	for (BlockSize = 2; BlockSize <= NumSamples; BlockSize <<= 1) {

		double delta_angle = angle_numerator / (double) BlockSize;

		double sm2 = sin(-2 * delta_angle);
		double sm1 = sin(-delta_angle);
		double cm2 = cos(-2 * delta_angle);
		double cm1 = cos(-delta_angle);
		double w = 2 * cm1;
		double ar0, ar1, ar2, ai0, ai1, ai2;

		for (i = 0; i < NumSamples; i += BlockSize) {
			ar2 = cm2;
			ar1 = cm1;

			ai2 = sm2;
			ai1 = sm1;

			for (j = i, n = 0; n < BlockEnd; j++, n++) {
				ar0 = w * ar1 - ar2;
				ar2 = ar1;
				ar1 = ar0;

				ai0 = w * ai1 - ai2;
				ai2 = ai1;
				ai1 = ai0;

				k = j + BlockEnd;
				tr = ar0 * RealOut[k] - ai0 * ImagOut[k];
				ti = ar0 * ImagOut[k] + ai0 * RealOut[k];

				RealOut[k] = RealOut[j] - tr;
				ImagOut[k] = ImagOut[j] - ti;

				RealOut[j] += tr;
				ImagOut[j] += ti;
			}
		}
		BlockEnd = BlockSize;
	}

	/*
	 **   Need to normalize if inverse transform...
	 */

	if (InverseTransform) {
		float denom = (float) NumSamples;

		for (i = 0; i < NumSamples; i++) {
			RealOut[i] /= denom;
			ImagOut[i] /= denom;
		}
	}
}

void InitFFT()
{
	gFFTBitTable = malloc(MaxFastBits*sizeof(int));

	int len = 2;
	int b, i;
	for (b=1; b<=MaxFastBits; b++) {
		gFFTBitTable[b-1]=malloc(len*sizeof(int));

		for (i=0; i<len; i++)
			gFFTBitTable[b-1][i] = ReverseBits(i, b);

		len <<= 1;
	}
}

int NumberOfBitsNeeded(int PowerOfTwo)
{
	int i;

/*
	if (PowerOfTwo < 2) {
		fprintf(stderr, "Error: FFT called with size %d\n", PowerOfTwo);
		exit(1);
	}
*/
	for (i = 0;; i++)
		if (PowerOfTwo & (1 << i))
			return i;
}

inline int FastReverseBits(int i, int NumBits)
{
	if (NumBits <= MaxFastBits)
		return gFFTBitTable[NumBits - 1][i];
	else
		return ReverseBits(i, NumBits);
}

int ReverseBits(int index, int NumBits)
{
   int i, rev;

   for (i = rev = 0; i < NumBits; i++) {
      rev = (rev << 1) | (index & 1);
      index >>= 1;
   }

   return rev;
}


void audio2hsv_1(gint audio_level, gint *light_h, gint *light_s, gint *light_v) {
	// Dummy code
	*light_h=-audio_level;
	*light_s=audio_level;
	*light_v=65535;
}

		
void hsv2rgb(gint h, gint s, gint v, gint *r, gint *g, gint *b) {
   /*
    * Purpose:
    * Convert HSV values to RGB values
    * All values are in the range [0..65535]
    */
   float F, M, N, K;
   int   I;
   
   if ( s == 0 ) {
      /* 
       * Achromatic case, set level of grey 
       */
      *r = v;
      *g = v;
      *b = v;
   } else {
      I = (int) h/(65535/6);	/* should be in the range 0..5 */
      F = h - I;		/* fractional part */

      M = v * (1 - s);
      N = v * (1 - s * F);
      K = v * (1 - s * (1 - F));

      if (I == 0) { *r = v; *g = K; *b = M; }
      if (I == 1) { *r = N; *g = v; *b = M; }
      if (I == 2) { *r = M; *g = v; *b = K; }
      if (I == 3) { *r = M; *g = N; *b = v; }
      if (I == 4) { *r = K; *g = M; *b = v; }
      if (I == 5) { *r = v; *g = M; *b = N; }
   }
}