#include <cstdio>
#include <algorithm>

#include "voicemanager.h"

#define VOICE_SCORE_MSB                 (8 * sizeof(score_t) - 1)

#define VOICE_SCORE_ACTIVE              (                     1 << (VOICE_SCORE_MSB - 0))
#define VOICE_SCORE_CHANNEL(channel)    ((score_t) (channel)    << (VOICE_SCORE_MSB - 4))
#define VOICE_SCORE_SERIAL(serial)      ((score_t) serial & ((1 << (VOICE_SCORE_MSB - 4)) - 1))

VoiceManager::VoiceManager() : serial(0) {
    for(VoiceData& vd : voices) {
        vd.channel = 0;
        vd.serial = 0;
    }
}

VoiceManager::score_t VoiceManager::scoreVoice(VoiceData& voiceData) {
    return
        (voiceData.voice.isIdle() ? 0 : VOICE_SCORE_ACTIVE) |
        VOICE_SCORE_CHANNEL(voiceData.channel) |
        VOICE_SCORE_SERIAL(voiceData.serial);
}

Voice * VoiceManager::get(channel_t channel) {
    // Sort idle voices first, then by highest channel and lowest serial
    std::sort(std::begin(voices), std::end(voices), [this](VoiceData& a, VoiceData& b) {
        return scoreVoice(a) < scoreVoice(b);
    });

/*
    putchar('\n');
    for(VoiceData& vd : voices) {
        printf("channel=%d serial=%d idle=%d note=%d score=%x\n", vd.channel, vd.serial, vd.voice.isIdle(), vd.voice.note, scoreVoice(vd));
    }
    putchar('\n');
*/

    VoiceData& voiceData = voices[0];
    if(!voiceData.voice.isIdle()) {
        printf("Out of voices\n");
    }
    voiceData.channel = channel;
    voiceData.serial = serial++;
    voiceData.voice.reset();

    return &voiceData.voice;
}

Voice** VoiceManager::getChannelVoices(channel_t channel) {
    static Voice* result[NUM_VOICES + 1];
    Voice** voice = result;
    bool empty = true;
    for(VoiceData& voiceData : voices) {
        if(!voiceData.voice.isIdle()) {
            empty = false;
            if(voiceData.channel == channel) {
                *voice++ = &voiceData.voice;
            }
        }
    }
    *voice = NULL;

    // Reset serial if voice table is empty
    if(empty) {
        serial = 0;
    }

    return result;
}