musiclight2/main.c

/*
 * vim: sw=2 ts=2 expandtab
 *
 * THE PIZZA-WARE LICENSE" (derived from "THE BEER-WARE LICENCE"):
 * <cfr34k@tkolb.de> wrote this file. As long as you retain this notice you can
 * do whatever you want with this stuff. If we meet some day, and you think
 * this stuff is worth it, you can buy me a pizza in return. - Thomas Kolb
 */

#include <pthread.h>
#include <semaphore.h>

#include <stdio.h>
#include <math.h>
#include <string.h>
#include <stdint.h>

#include "fft.h"
#include "utils.h"
#include "ws2801.h"

#include "config.h"

// Frames per second
#define FPS ((double)BUFFER_PARTS * SAMPLE_RATE / BLOCK_LEN)

// Number of new samples put into the buffer each frame
#define READ_SAMPLES (BLOCK_LEN / BUFFER_PARTS)

#define COLORBUF_SIZE (2*(NUM_MODULES+1))
#define CENTER_POS (2*CENTER_MODULE)

double fft[BLOCK_LEN];
double rms;
double redEnergy, greenEnergy, blueEnergy;
double lastUpdateTime = 0;

sem_t  fftSemaphore;

int running = 1;

void* fft_thread(void *param) {
	sample       buffer[BLOCK_LEN];
	sample       block[BLOCK_LEN];
	double       fftOutReal[BLOCK_LEN], fftOutImag[BLOCK_LEN];
	double       tmpFFT[BLOCK_LEN];
  double       tmpRMS;

	double       nextFrame = get_hires_time() + 0.05;
	double       curTime;

	int i;

	init_fft();

	while(running) {
		// shift the buffer left
		memmove(buffer, buffer + READ_SAMPLES,
		        (BLOCK_LEN - READ_SAMPLES) * sizeof(sample));

		size_t ret = fread(buffer + (BLOCK_LEN - READ_SAMPLES),
		                   sizeof(sample), READ_SAMPLES, stdin);
		if(ret != READ_SAMPLES) {
			break;
		}

		memcpy(block, buffer, BLOCK_LEN * sizeof(sample));

		apply_hanning(block);
		fft_transform(block, fftOutReal, fftOutImag);
		complex_to_absolute(fftOutReal, fftOutImag, tmpFFT);

    tmpRMS = 0;
    for(i = 0; i < BLOCK_LEN; i++) {
      tmpRMS += block[i]*block[i];
    }
    tmpRMS = sqrt(tmpRMS/BLOCK_LEN);

    // --- SAFE SECTION ---
    sem_wait(&fftSemaphore);

    memcpy(fft, tmpFFT, sizeof(fft));
    rms = tmpRMS;
    redEnergy = get_energy_in_band(fft, RED_MIN_FREQ, RED_MAX_FREQ);
    greenEnergy = get_energy_in_band(fft, GREEN_MIN_FREQ, GREEN_MAX_FREQ);
    blueEnergy = get_energy_in_band(fft, BLUE_MIN_FREQ, BLUE_MAX_FREQ);

		curTime = get_hires_time();
    lastUpdateTime = curTime;

		sem_post(&fftSemaphore);
    // --- END SAFE SECTION ---

		if(curTime > nextFrame + 0.05) {
			printf("Frame too late! Skipping.\n");
			nextFrame = -1;
		}

		if(curTime < nextFrame - 0.05) {
			printf("Frame too early.\n");
			nextFrame = -1;
		}

		if(nextFrame < 0) {
			printf("Frame time reset.\n");
			nextFrame = curTime;
		}

		nextFrame += 1.000/FPS;
		sleep_until(nextFrame);
	}

	return NULL;
}

double gamma_correct(double d) {
  return pow(d, GAMMA);
}

void text_bar(double fill) {
  int fillCnt = 10 * fill;
  int i;

  for(i = 0; i < fillCnt; i++) {
    printf("|");
  }

  for(; i < 10; i++) {
    printf("-");
  }
}

double weighted_avg(uint8_t colorBuf[COLORBUF_SIZE][3], int channel, int centerPos) {
  return 0.20 * colorBuf[centerPos - 1][channel] +
         0.60 * colorBuf[centerPos][channel] +
         0.20 * colorBuf[centerPos + 1][channel];
}

void show_status(double curRed, double maxRed, double curGreen, double maxGreen, double curBlue, double maxBlue) {
  printf("\r");

  printf("[\033[31m");
  text_bar(curRed / maxRed);
  printf("\033[0m] ");
  printf("\033[1;31m%7.2e\033[0m ", maxRed / (RED_MAX_FREQ - RED_MIN_FREQ));

  printf("[\033[32m");
  text_bar(curGreen / maxGreen);
  printf("\033[0m] ");
  printf("\033[1;32m%7.2e\033[0m ", maxGreen / (GREEN_MAX_FREQ - GREEN_MIN_FREQ));

  printf("[\033[34m");
  text_bar(curBlue / maxBlue);
  printf("\033[0m] ");
  printf("\033[1;34m%7.2e\033[0m ", maxBlue / (BLUE_MAX_FREQ - BLUE_MIN_FREQ));

  fflush(stdout);
}

int main(int argc, char **argv) {
  double nextFrame = get_hires_time() + LED_INTERVAL;

	int i, j;
	pthread_t fftThread;

  int active = 1;

  uint8_t colorBuf[COLORBUF_SIZE][3];

  double curRedEnergy, curGreenEnergy, curBlueEnergy;
  double maxRedEnergy = 1, maxGreenEnergy = 1, maxBlueEnergy = 1;

  memset(colorBuf, 0, sizeof(colorBuf));

	// create semaphores
	sem_init(&fftSemaphore, 0, 1);

	// run the fft thread
	pthread_create(&fftThread, NULL, fft_thread, NULL);

  ws2801_init(HOST, PORT);

	while(running) {
    for(i = COLORBUF_SIZE-1; i >= 0; i--) {
      int pos = CENTER_POS + i;
      if(pos < COLORBUF_SIZE-1) {
        for(j = 0; j < 3; j++) {
          colorBuf[pos][j] = colorBuf[pos - 1][j];
        }
      }

      pos = CENTER_POS - i;
      if(pos >= 0) {
        for(j = 0; j < 3; j++) {
          colorBuf[pos][j] = colorBuf[pos + 1][j];
        }
      }
    }

    if(active) {
      sem_wait(&fftSemaphore);
      curRedEnergy = redEnergy;
      curGreenEnergy = greenEnergy;
      curBlueEnergy = blueEnergy;
      sem_post(&fftSemaphore);

      maxRedEnergy *= COLOR_MAX_REDUCTION_FACTOR;
      if(curRedEnergy > maxRedEnergy) {
        maxRedEnergy = curRedEnergy;
      }

      maxGreenEnergy *= COLOR_MAX_REDUCTION_FACTOR;
      if(curGreenEnergy > maxGreenEnergy) {
        maxGreenEnergy = curGreenEnergy;
      }

      maxBlueEnergy *= COLOR_MAX_REDUCTION_FACTOR;
      if(curBlueEnergy > maxBlueEnergy) {
        maxBlueEnergy = curBlueEnergy;
      }

      colorBuf[CENTER_POS][0] = 255 * gamma_correct(curRedEnergy / maxRedEnergy);
      colorBuf[CENTER_POS][1] = 255 * gamma_correct(curGreenEnergy / maxGreenEnergy);
      colorBuf[CENTER_POS][2] = 255 * gamma_correct(curBlueEnergy / maxBlueEnergy);

      /*
      show_status(curRedEnergy, maxRedEnergy, curGreenEnergy, maxGreenEnergy,
          curBlueEnergy, maxBlueEnergy);
      */

      for(i = 0; i < NUM_MODULES; i++) {
        ws2801_set_color(i,
            weighted_avg(colorBuf, 0, 2 * i + 1),
            weighted_avg(colorBuf, 1, 2 * i + 1),
            weighted_avg(colorBuf, 2, 2 * i + 1));
      }
      ws2801_commit();

      if(lastUpdateTime < nextFrame - 1) {
        printf("Idle for 1 second -> stopping updates.\n");

        for(i = 0; i < NUM_MODULES; i++) {
          ws2801_fade_color(i, 20, 20, 20);
        }
        ws2801_commit();

        active = 0;
      }
    } else if(lastUpdateTime > nextFrame - 1) {
      printf("Resuming updates.\n");
      active = 1;
    }

    nextFrame += LED_INTERVAL;
    sleep_until(nextFrame);
	}

  ws2801_shutdown();

	pthread_join(fftThread, NULL);

	return 0;
}