   usbMIDI.read(); // USB MIDI receive
 }
 
+void initVoice(byte voice) {
+  switch (voice) {
+    case 1:
+      initSine(&sine1);
+      initSine(&sine2);
+      initSine(&sine3);
+      initEnvelope(&envelope1);
+      initMixer(&mixer1);
+      break;
+    case 2:
+      initSine(&sine4);
+      initSine(&sine5);
+      initSine(&sine6);
+      initEnvelope(&envelope2);
+      initMixer(&mixer2);
+      break;
+    case 3:
+      initSine(&sine7);
+      initSine(&sine8);
+      initSine(&sine9);
+      initEnvelope(&envelope3);
+      initMixer(&mixer3);
+      break;
+    case 4:
+      initSine(&sine10);
+      initSine(&sine11);
+      initSine(&sine12);
+      initEnvelope(&envelope4);
+      initMixer(&mixer4);
+      break;
+  }
+}
+
+void initSine(AudioSynthWaveformSine *sine) {
+  sine->amplitude(1.0);
+}
+
+void initEnvelope(AudioEffectEnvelope *env) {
+  env->attack(0.2);
+  env->sustain(1.0);
+  env->release(0.1);
+}
+
+void initMixer(AudioMixer4 *mix) {
+  mix->gain(0, 0.083);
+  mix->gain(1, 0.083);
+  mix->gain(2, 0.083);
+  mix->gain(3, 0);
+}
+
+byte inuse;
+byte voices[127];
+
+byte findVoice() {
+  if (inuse != 0xFF) {
+    for (int i=0; i<4; i++) {
+      byte pos = 1<<i;
+      if ((inuse & pos) == 0) {
+        return i+1;
+      }
+    }
+  }
+
+  // Lowest notes have priority for now.
+  for (byte i=127; i>=0; i--) {
+    if (voices[i] != 0) {
+      returnVoice(i);
+      return i;
+    }
+  }
+}
+
+byte allocateVoice(byte note) {
+  byte voice = findVoice();
+  
+  inuse |= (1 << (voice-1));
+
+  Serial.print("Voice=");
+  Serial.print(voice);
+  Serial.print(", inuse=");
+  Serial.print(inuse);
+  Serial.println();
+  
+  voices[note] = voice;
+  return voice;
+}
+
+void returnVoice(byte note) {
+  byte voice = voices[note];
+  inuse &= ~(1 << (voice-1)); 
+
+  Serial.print("Voice=");
+  Serial.print(voice);
+  Serial.print(", inuse=");
+  Serial.print(inuse);
+  Serial.println();
+  
+  voices[note] = 0;
+}
 
 void OnNoteOn(byte channel, byte note, byte velocity) {
   Serial.print("Note On, ch=");

  Serial.print(velocity, DEC);
  Serial.println();

  sine1.frequency(note2freq(note));
  sine1.amplitude(0.5);
}

void OnNoteOff(byte channel, byte note, byte velocity) {

  Serial.print(velocity, DEC);
  Serial.println();

  byte voice = voices[note];
  switch (voice) {
    case 1:
      envelope1.noteOff();
      returnVoice(note);
      break;
    case 2:
      envelope2.noteOff();
      returnVoice(note);
      break;
    case 3:
      envelope3.noteOff();
      returnVoice(note);
      break;
    case 4:
      envelope4.noteOff();
      returnVoice(note);
      break;
  }
}

void OnVelocityChange(byte channel, byte note, byte velocity) {
