#include <GD.h>

#define GAIN    0x2807
#define VOICE0F 0x2a00
#define VOICE0A 0x2a02
#define VOICE0W 0x2a03

struct voice
{
  float f;
  float a;
} voices[16];

// counter 1000, freq ~188Hz
// 1000, increments every 
//  f = (c/65536)*(1./2**12)*(50e6)
// c = (f * 65536) / ((1./2**12)*(50e6))
#define CNT(F) int(((F) * 65536.)/(((1./4096)*(50e6))))

#define PAUSE(x)      255,x
#define NOTE(v, p, a) v, p, a

static PROGMEM prog_uchar widor_toccata[] = {
  #include "music.i"
};

void load()
{
  byte v;
  unsigned int gg = 0;
  for (v = 0; v < 16; v++) {
    GD.sprite(v,
      int(400 * log(voices[v].f) / 9) - 8,
      int(280 - 260 * voices[v].a) - 8,
      0, 0, 0);
    GD.wr16(VOICE0F + (v*4), CNT(voices[v].f));
    byte a = int(255 * voices[v].a);
    GD.wr(VOICE0A + (v*4), a);
    gg += a;
    GD.wr(VOICE0W + (v*4), 0);
  }
  GD.wr(GAIN, min(255, int(16 * 4096. / gg)));
}

void note(byte voice, byte m, byte vel)
{
  float f0 = 440 * pow(2.0, (m - 69) / 12.0);
  float a0 = vel / 120.;
  if (voice == 0) {
    float choirA[] = { 3.5, 1.6, .7, 3.7, 1, 2 };
    byte v;
    for (v = 0; v < 6; v++) {
      voices[v].f = (v + 1) * f0;
      voices[v].a = a0 * choirA[v] / 3.7;
    }
  } else {
    voices[voice].f = f0;
    voices[voice].a = a0;
  }
}

static void pause(int n)
{
  load();
  delay(n * 3 / 2);
}

static void play()
{
  prog_uchar *pc = widor_toccata;
  while (pc < (widor_toccata + sizeof(widor_toccata))) {
    byte a = pgm_read_byte_near(pc++);
    byte b = pgm_read_byte_near(pc++);
    if (a == 255) {
      pause(b);
    } else {
      byte c = pgm_read_byte_near(pc++);
      note(a, b, c);
    }
  }
}

void setup()
{
  int i;

  GD.begin();
  
  Serial.begin(115200);
  Serial.println("");
  Serial.println("");

  GD.ascii();
  GD.fill(0, ' ', 4096);

  GD.wr16(RAM_SPRPAL + (0 * 2), 0x8000);
  GD.wr16(RAM_SPRPAL + (1 * 2), RGB(255, 255, 255));

  GD.wr(RAM_SPRIMG + 0x78, 1);
  GD.wr(RAM_SPRIMG + 0x98, 1);
  GD.wr(RAM_SPRIMG + 0x87, 1);
  GD.wr(RAM_SPRIMG + 0x89, 1);
  GD.wr(RAM_SPRIMG + 0x88, 1);

  for (i = 0; i < 256; i++)
    GD.sprite(i, 400, 400, 0, 0, 0);

  GD.putstr(0, 0,"Widor's Toccata");
  byte v;

  for (v = 0; v < 16; v++) {
    voices[v].f = 0.0;
    voices[v].a = 0.0;
  }
  if (0) {
    // Choir
    float choirA[] = { .5, 1.6, .7, 3.7, 0, 2 };
    float choirB[] = { .5, .6, 0, 2 };
    // float f0 = 261.63;
    // float f1 = 329.63;
    float f0 = 329.63;
    float f1 = 369.99;
    for (v = 0; v < 6; v++) {
      voices[v].f = (v + 1) * f0;
      voices[v].a = choirA[v] / 3.7;
    }
    if (1)
      for (v = 0; v < 4; v++) {
        voices[8 + v].f = (v + 1) * f1;
        voices[8 + v].a = choirB[v] / 3.7;
      }
  }
  if (0) {
    for (v = 0; v < 16; v++) {
      voices[v].f = random(100,800);
      voices[v].a = 1.0;
    }
  }
#if 0
  note(0, 65, 70);
  note(1, 69, 70);
  note(2, 72, 70);
  note(3, 77, 70);
  load();
  for (;;) {
    delay(128);
    note(4, 88, 91);
    load();
    delay(128);
    note(4, 88, 0);
    load();
  }
#else
  play();
#endif
}

void whitenoise()
{
  byte v;
  float center = 4 + 2 * sin((millis() / 1000.));
  for (v = 12; v < 16; v++) {
    int lo = pow(2.0, center - 0.5);
    int hi = pow(2.0, center + 0.5);
    voices[v].f = random(lo, hi);
    voices[v].a = 0.0;
    GD.wr(VOICE0W + (v*4), 1);
  }
  load();
}

void xloop()
{
  byte v;
  float t = (millis() % 1000) / 1000.;
  int partials = 16;
  for (v = 0; v < partials; v++) {
    voices[v].f = 16.35 * pow(1.4, v + t);
    voices[v].a = sin(3.1415 * (v + t) / partials);
    // Serial.print(v, DEC); Serial.print(" ");
    // Serial.print(voices[v].f); Serial.print(" "); Serial.println(voices[v].a);
  }
  // Serial.println(voices[0].f);
  load();
}

void loop()
{
}
