gstreamer/gst/synaesthesia/synaescope.c

/*  synaescope.cpp
 *  Copyright (C) 1999,2002 Richard Boulton <richard@tartarus.org>
 *
 *  Much code copied from Synaesthesia - a program to display sound
 *  graphically, by Paul Francis Harrison <pfh@yoyo.cc.monash.edu.au>
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include "synaescope.h"

#include <pthread.h>
#include <dirent.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <time.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <assert.h>

#define SCOPE_BG_RED 0
#define SCOPE_BG_GREEN 0
#define SCOPE_BG_BLUE 0

#define FFT_BUFFER_SIZE_LOG 9
#define FFT_BUFFER_SIZE (1 << FFT_BUFFER_SIZE_LOG)

#define syn_width 320
#define syn_height 200
#define brightMin 200
#define brightMax 2000
#define brightDec 10
#define brightInc 6
#define brTotTargetLow 5000
#define brTotTargetHigh 15000

static int autobrightness = 1;  /* Whether to use automatic brightness adjust */
static unsigned int brightFactor = 400;
static unsigned char output[syn_width * syn_height * 2];
static guint32 display[syn_width * syn_height];
static gint16 pcmt_l[FFT_BUFFER_SIZE];
static gint16 pcmt_r[FFT_BUFFER_SIZE];
static gint16 pcm_l[FFT_BUFFER_SIZE];
static gint16 pcm_r[FFT_BUFFER_SIZE];
static double fftout_l[FFT_BUFFER_SIZE];
static double fftout_r[FFT_BUFFER_SIZE];
static double fftmult[FFT_BUFFER_SIZE / 2 + 1];
static double corr_l[FFT_BUFFER_SIZE];
static double corr_r[FFT_BUFFER_SIZE];
static int clarity[FFT_BUFFER_SIZE];    /* Surround sound */
static double cosTable[FFT_BUFFER_SIZE];
static double negSinTable[FFT_BUFFER_SIZE];
static int bitReverse[FFT_BUFFER_SIZE];
static int scaleDown[256];

static void synaes_fft (double *x, double *y);
static void synaescope_coreGo (void);

#define SYNAESCOPE_DOLOOP() \
while (running) { \
    gint bar; \
    guint val; \
    gint val2; \
    unsigned char *outptr = output; \
        int w; \
\
    synaescope_coreGo(); \
\
    outptr = output; \
    for (w=0; w < syn_width * syn_height; w++) { \
        bits[w] = colEq[(outptr[0] >> 4) + (outptr[1] & 0xf0)]; \
        outptr += 2; \
    } \
\
    GDK_THREADS_ENTER(); \
    gdk_draw_image(win,gc,image,0,0,0,0,-1,-1); \
    gdk_flush(); \
    GDK_THREADS_LEAVE(); \
    dosleep(SCOPE_SLEEP); \
}

static inline void
addPixel (unsigned char *output, int x, int y, int br1, int br2)
{
  unsigned char *p;

  if (x < 0 || x >= syn_width || y < 0 || y >= syn_height)
    return;

  p = output + x * 2 + y * syn_width * 2;
  if (p[0] < 255 - br1)
    p[0] += br1;
  else
    p[0] = 255;
  if (p[1] < 255 - br2)
    p[1] += br2;
  else
    p[1] = 255;
}

static inline void
addPixelFast (unsigned char *p, int br1, int br2)
{
  if (p[0] < 255 - br1)
    p[0] += br1;
  else
    p[0] = 255;
  if (p[1] < 255 - br2)
    p[1] += br2;
  else
    p[1] = 255;
}

static void
synaescope_coreGo (void)
{
  int i, j;
  register unsigned long *ptr;
  register unsigned long *end;
  int heightFactor;
  int actualHeight;
  int heightAdd;
  double brightFactor2;
  long int brtot;

  memcpy (pcm_l, pcmt_l, sizeof (pcm_l));
  memcpy (pcm_r, pcmt_r, sizeof (pcm_r));

  for (i = 0; i < FFT_BUFFER_SIZE; i++) {
    fftout_l[i] = pcm_l[i];
    fftout_r[i] = pcm_r[i];
  }

  synaes_fft (fftout_l, fftout_r);

  for (i = 0 + 1; i < FFT_BUFFER_SIZE; i++) {
    double x1 = fftout_l[bitReverse[i]];
    double y1 = fftout_r[bitReverse[i]];
    double x2 = fftout_l[bitReverse[FFT_BUFFER_SIZE - i]];
    double y2 = fftout_r[bitReverse[FFT_BUFFER_SIZE - i]];
    double aa, bb;

    corr_l[i] = sqrt (aa = (x1 + x2) * (x1 + x2) + (y1 - y2) * (y1 - y2));
    corr_r[i] = sqrt (bb = (x1 - x2) * (x1 - x2) + (y1 + y2) * (y1 + y2));
    clarity[i] = (int) (
        ((x1 + x2) * (x1 - x2) + (y1 + y2) * (y1 - y2)) / (aa + bb) * 256);
  }

  /* Asger Alstrupt's optimized 32 bit fade */
  /* (alstrup@diku.dk) */
  ptr = (unsigned long *) output;
  end = (unsigned long *) (output + syn_width * syn_height * 2);
  do {
    /*Bytewize version was: *(ptr++) -= *ptr+(*ptr>>1)>>4; */
    if (*ptr) {
      if (*ptr & 0xf0f0f0f0) {
        *ptr = *ptr - ((*ptr & 0xf0f0f0f0) >> 4) - ((*ptr & 0xe0e0e0e0) >> 5);
      } else {
        *ptr = (*ptr * 14 >> 4) & 0x0f0f0f0f;
        /*Should be 29/32 to be consistent. Who cares. This is totally */
        /* hacked anyway.  */
        /*unsigned char *subptr = (unsigned char*)(ptr++); */
        /*subptr[0] = (int)subptr[0] * 29 / 32; */
        /*subptr[1] = (int)subptr[0] * 29 / 32; */
        /*subptr[2] = (int)subptr[0] * 29 / 32; */
        /*subptr[3] = (int)subptr[0] * 29 / 32; */
      }
    }
    ptr++;
  } while (ptr < end);

  heightFactor = FFT_BUFFER_SIZE / 2 / syn_height + 1;
  actualHeight = FFT_BUFFER_SIZE / 2 / heightFactor;
  heightAdd = (syn_height + actualHeight) >> 1;

  /* Correct for window size */
  brightFactor2 = (brightFactor / 65536.0 / FFT_BUFFER_SIZE) *
      sqrt (actualHeight * syn_width / (320.0 * 200.0));

  brtot = 0;
  for (i = 1; i < FFT_BUFFER_SIZE / 2; i++) {
    /*int h = (int)( corr_r[i]*280 / (corr_l[i]+corr_r[i]+0.0001)+20 ); */
    if (corr_l[i] > 0 || corr_r[i] > 0) {
      int h = (int) (corr_r[i] * syn_width / (corr_l[i] + corr_r[i]));

/*      int h = (int)( syn_width - 1 ); */
      int br1, br2, br = (int) ((corr_l[i] + corr_r[i]) * i * brightFactor2);
      int px = h, py = heightAdd - i / heightFactor;

      brtot += br;
      br1 = br * (clarity[i] + 128) >> 8;
      br2 = br * (128 - clarity[i]) >> 8;
      if (br1 < 0)
        br1 = 0;
      else if (br1 > 255)
        br1 = 255;
      if (br2 < 0)
        br2 = 0;
      else if (br2 > 255)
        br2 = 255;
      /*unsigned char *p = output+ h*2+(164-((i<<8)>>FFT_BUFFER_SIZE_LOG))*(syn_width*2);  */

      if (px < 30 || py < 30 || px > syn_width - 30 || py > syn_height - 30) {
        addPixel (output, px, py, br1, br2);
        for (j = 1; br1 > 0 || br2 > 0;
            j++, br1 = scaleDown[br1], br2 = scaleDown[br2]) {
          addPixel (output, px + j, py, br1, br2);
          addPixel (output, px, py + j, br1, br2);
          addPixel (output, px - j, py, br1, br2);
          addPixel (output, px, py - j, br1, br2);
        }
      } else {
        unsigned char *p = output + px * 2 + py * syn_width * 2, *p1 = p, *p2 =
            p, *p3 = p, *p4 = p;
        addPixelFast (p, br1, br2);
        for (; br1 > 0 || br2 > 0; br1 = scaleDown[br1], br2 = scaleDown[br2]) {
          p1 += 2;
          addPixelFast (p1, br1, br2);
          p2 -= 2;
          addPixelFast (p2, br1, br2);
          p3 += syn_width * 2;
          addPixelFast (p3, br1, br2);
          p4 -= syn_width * 2;
          addPixelFast (p4, br1, br2);
        }
      }
    }
  }

  /* Apply autoscaling: makes quiet bits brighter, and loud bits
   * darker, but still keeps loud bits brighter than quiet bits. */
  if (brtot != 0 && autobrightness) {
    long int brTotTarget = brTotTargetHigh;

    if (brightMax != brightMin) {
      brTotTarget -= ((brTotTargetHigh - brTotTargetLow) *
          (brightFactor - brightMin)) / (brightMax - brightMin);
    }
    if (brtot < brTotTarget) {
      brightFactor += brightInc;
      if (brightFactor > brightMax)
        brightFactor = brightMax;
    } else {
      brightFactor -= brightDec;
      if (brightFactor < brightMin)
        brightFactor = brightMin;
    }
    /* printf("brtot: %ld\tbrightFactor: %d\tbrTotTarget: %d\n",
       brtot, brightFactor, brTotTarget); */
  }
}

#define BOUND(x) ((x) > 255 ? 255 : (x))
#define PEAKIFY(x) BOUND((x) - (x)*(255-(x))/255/2)

static void
synaescope32 ()
{
  unsigned char *outptr;
  guint32 colEq[256];
  int i;
  guint32 bg_color;

  for (i = 0; i < 256; i++) {
    int red = PEAKIFY ((i & 15 * 16));
    int green = PEAKIFY ((i & 15) * 16 + (i & 15 * 16) / 4);
    int blue = PEAKIFY ((i & 15) * 16);

    colEq[i] = (red << 16) + (green << 8) + blue;
  }
  bg_color = (SCOPE_BG_RED << 16) + (SCOPE_BG_GREEN << 8) + SCOPE_BG_BLUE;

  synaescope_coreGo ();

  outptr = output;
  for (i = 0; i < syn_width * syn_height; i++) {
    display[i] = colEq[(outptr[0] >> 4) + (outptr[1] & 0xf0)];
    outptr += 2;
  }
}


#if 0
static void
synaescope16 (void *data)
{
  guint16 *bits;
  guint16 colEq[256];
  int i;
  GdkWindow *win;
  GdkColormap *c;
  GdkVisual *v;
  GdkGC *gc;
  GdkColor bg_color;

  win = (GdkWindow *) data;
  GDK_THREADS_ENTER ();
  c = gdk_colormap_get_system ();
  gc = gdk_gc_new (win);
  v = gdk_window_get_visual (win);

  for (i = 0; i < 256; i++) {
    GdkColor color;

    color.red = PEAKIFY ((i & 15 * 16)) << 8;
    color.green = PEAKIFY ((i & 15) * 16 + (i & 15 * 16) / 4) << 8;
    color.blue = PEAKIFY ((i & 15) * 16) << 8;
    gdk_color_alloc (c, &color);
    colEq[i] = color.pixel;
  }

  /* Create render image */
  if (image) {
    gdk_image_destroy (image);
    image = NULL;
  }
  image = gdk_image_new (GDK_IMAGE_FASTEST, v, syn_width, syn_height);
  bg_color.red = SCOPE_BG_RED << 8;
  bg_color.green = SCOPE_BG_GREEN << 8;
  bg_color.blue = SCOPE_BG_BLUE << 8;
  gdk_color_alloc (c, &bg_color);
  GDK_THREADS_LEAVE ();

  assert (image);
  assert (image->bpp == 2);

  bits = (guint16 *) image->mem;

  running = 1;

  SYNAESCOPE_DOLOOP ();
}


static void
synaescope8 (void *data)
{
  unsigned char *outptr;
  guint8 *bits;
  guint8 colEq[256];
  int i;
  GdkWindow *win;
  GdkColormap *c;
  GdkVisual *v;
  GdkGC *gc;
  GdkColor bg_color;

  win = (GdkWindow *) data;
  GDK_THREADS_ENTER ();
  c = gdk_colormap_get_system ();
  gc = gdk_gc_new (win);
  v = gdk_window_get_visual (win);

  for (i = 0; i < 64; i++) {
    GdkColor color;

    color.red = PEAKIFY ((i & 7 * 8) * 4) << 8;
    color.green = PEAKIFY ((i & 7) * 32 + (i & 7 * 8) * 2) << 8;
    color.blue = PEAKIFY ((i & 7) * 32) << 8;
    gdk_color_alloc (c, &color);
    colEq[i * 4] = color.pixel;
    colEq[i * 4 + 1] = color.pixel;
    colEq[i * 4 + 2] = color.pixel;
    colEq[i * 4 + 3] = color.pixel;
  }

  /* Create render image */
  if (image) {
    gdk_image_destroy (image);
    image = NULL;
  }
  image = gdk_image_new (GDK_IMAGE_FASTEST, v, syn_width, syn_height);
  bg_color.red = SCOPE_BG_RED << 8;
  bg_color.green = SCOPE_BG_GREEN << 8;
  bg_color.blue = SCOPE_BG_BLUE << 8;
  gdk_color_alloc (c, &bg_color);
  GDK_THREADS_LEAVE ();

  assert (image);
  assert (image->bpp == 1);

  bits = (guint8 *) image->mem;

  running = 1;

  SYNAESCOPE_DOLOOP ();
}
#endif

#if 0
static void
run_synaescope (void *data)
{
  switch (depth) {
    case 8:
      synaescope8 (win);
      break;
    case 16:
      synaescope16 (win);
      break;
    case 24:
    case 32:
      synaescope32 (win);
      break;

  }
}

static void
start_synaescope (void *data)
{
  init_synaescope_window ();
}

#endif


static int
bitReverser (int i)
{
  int sum = 0;
  int j;

  for (j = 0; j < FFT_BUFFER_SIZE_LOG; j++) {
    sum = (i & 1) + sum * 2;
    i >>= 1;
  }

  return sum;
}

static void
init_synaescope ()
{
  int i;

  for (i = 0; i <= FFT_BUFFER_SIZE / 2 + 1; i++) {
    double mult = (double) 128 / ((FFT_BUFFER_SIZE * 16384) ^ 2);

    /* Result now guaranteed (well, almost) to be in range 0..128 */

    /* Low values represent more frequencies, and thus get more */
    /* intensity - this helps correct for that. */
    mult *= log (i + 1) / log (2);

    mult *= 3;                  /* Adhoc parameter, looks about right for me. */

    fftmult[i] = mult;
  }

  for (i = 0; i < FFT_BUFFER_SIZE; i++) {
    negSinTable[i] = -sin (M_PI * 2 / FFT_BUFFER_SIZE * i);
    cosTable[i] = cos (M_PI * 2 / FFT_BUFFER_SIZE * i);
    bitReverse[i] = bitReverser (i);
  }

  for (i = 0; i < 256; i++)
    scaleDown[i] = i * 200 >> 8;

  memset (output, 0, syn_width * syn_height * 2);
}

static void
synaes_fft (double *x, double *y)
{
  int n2 = FFT_BUFFER_SIZE;
  int n1;
  int twoToTheK;
  int j;

  for (twoToTheK = 1; twoToTheK < FFT_BUFFER_SIZE; twoToTheK *= 2) {
    n1 = n2;
    n2 /= 2;
    for (j = 0; j < n2; j++) {
      double c = cosTable[j * twoToTheK & (FFT_BUFFER_SIZE - 1)];
      double s = negSinTable[j * twoToTheK & (FFT_BUFFER_SIZE - 1)];
      int i;

      for (i = j; i < FFT_BUFFER_SIZE; i += n1) {
        int l = i + n2;
        double xt = x[i] - x[l];
        double yt = y[i] - y[l];

        x[i] = (x[i] + x[l]);
        y[i] = (y[i] + y[l]);
        x[l] = xt * c - yt * s;
        y[l] = xt * s + yt * c;
      }
    }
  }
}

static void
synaescope_set_data (gint16 data[2][512])
{
  int i;
  gint16 *newset_l = pcmt_l;
  gint16 *newset_r = pcmt_r;

  for (i = 0; i < FFT_BUFFER_SIZE; i++) {
    newset_l[i] = data[0][i];
    newset_r[i] = data[1][i];
  }
}

void
synaesthesia_init (guint32 resx, guint32 resy)
{
  init_synaescope ();
}

guint32 *
synaesthesia_update (gint16 data[2][512])
{
  synaescope_set_data (data);
  synaescope32 ();
  return display;
}

void
synaesthesia_close ()
{
}