#include "YM3812.h"
//Arduino Driver for OPL2 (YM3812) Version 0.2 (03/04/2016) **NO TEST**
//Warning Binary data are on array -> {bit 0,bit 1, ..., bit 7}

short IC, RD, WR, AO, D0;
short tempData[8] = {0, 0, 0, 0, 0, 0, 0, 0}; //Function already use this for communicate array
bool rhythmic = false;

//General Function
void YM3812::init(String mode){
    //Pin definition for Arduino
    IC = 2;
    RD = 3;
    WR = 4;
    AO = 5;
    D0 = 6; //D0 begin of Data bus (D1 = D0+1, D2 = D0+2, ..., D7 = D0+7)
    clearRegister();

    if(mode == "RHYTHMIC"){
        rhythmic = true;
        enableRhythmic();
    }
    if(mode == "MELODY"){
        rhythmic = false;
    }

    //Set default configuration
    for(short Operator = 1; Operator <= 2; Operator++){
        for(short voice = 1; voice <= 9; voice++){
            attackDecay(voice,Operator,10,10);
            sustainRelease(voice,Operator,10,10);
            level(voice, Operator,0);
            waveform(voice, Operator, 0);

        }
    }
}

void YM3812::clearRegister(){
    digitalWrite(IC, 0);
    delayMicroseconds(250);
    digitalWrite(IC, 1);
}

void YM3812::test(){
    short testAddress[8] = {1, 0, 0, 0, 0, 0, 0, 0};
    short testData[8] = {0, 0, 0, 0, 1, 0, 0, 0};
    send(testAddress,testData);
}

void YM3812::send(short address[], short data[]){ //Send 8bits control signal to OPL
    digitalWrite(RD, 1); //Write Address
    for(short i = D0+7; i >= D0; i-1){
        short buffAddress = address[i-D0+7];
        digitalWrite(i, buffAddress);
    }
    digitalWrite(RD, 0);

    digitalWrite(RD, 1); //Write Data
    digitalWrite(A0, 1);
    for(short i = D0+7; i >= D0; i-1){
        short buffData = data[i-D0+7];
        digitalWrite(i, buffData);
    }
    digitalWrite(RD, 0);
    digitalWrite(A0, 0);

    digitalWrite(WR, 1); //Enable READ instruction
    delayMicroseconds(250);
    digitalWrite(WR, 0);
}

void YM3812::copyArrays(short array1[], short array2[],short start, short length){
    for(short i = start; i <= length-1; i++){
        array2[i] = array1[i];
    }
}

void YM3812::addArrays(short array1[], short array2[]){
    for(short i = 0; i <= 7; i++){
        if(array1[i] + array2[i] == 0){
            array2[i] = 0;
        }
        else if(array1[i] + array2 [i] > 1){
            array2[i] = 0;
            array2[i+1] = 1;
        }
        else
            array2[i] = 1;

    }

}

void YM3812::decimalBinArray(short decimal){
    short binary[8], residual, value;
    for(short i = 0; i <= 7; i++){
        residual = decimal % 2 ;
        binary[i] = residual;
        value = value/2;
    }
    copyArrays(binary,tempData, 0, 8) ;
}

void YM3812::addressCalculator(short voice, short Operator){
    if(Operator == 1){
        //Address calculator
        if(voice <= 3){//Voices 1,2,3
            decimalBinArray(voice-1);
        }
        else if(voice <= 6){//Voices 4,5,6
            decimalBinArray(voice+4);
        }
        else if(voice <= 9){//Voice 7,8,9
            decimalBinArray(voice+6);
        }
    }
    else if(Operator == 2){
        //Address calculator
        if(voice <= 3){//Voices 1,2,3
            decimalBinArray(voice+2);
        }
        else if(voice <= 6){//Voices 4,5,6
            decimalBinArray(voice+7);
        }
        else if(voice <= 9){//Voice 7,8,9
            decimalBinArray(voice+9);
        }
    }
}

short YM3812::numberVoicesLimiter(short voice){
    if(rhythmic){
        if(voice >= 6)
            return 0;
        else
            return voice;
    }
    else return voice;
}


//Function used for control voice
void YM3812::waveform(short voice, short Operator,short waveformType){
    short waveData[8] = {0, 0, 0, 0, 0, 0, 0, 0};
    short voiceAddress[8], registerAddress[8] = {0, 0, 0, 0, 0, 1, 1, 1};

    voice = numberVoicesLimiter(voice);

    switch(waveformType){
    case 1: //Sin wave
        break;
    case 2: //positive Half Sin wave
        waveData[0] = 1;
        break;
    case 3: //Double positive half sin wave
        waveData[1] = 1;
        break;
    case 4: //sawtooth
        waveData[0] = 1;
        waveData[1] = 1;
        break;
    }

    addressCalculator(voice, Operator);
    copyArrays(tempData, voiceAddress,0,8);
    addArrays(voiceAddress, registerAddress);
    send(registerAddress,waveData);
}

void YM3812::level(short voice, short Operator, short levelRate){
    short levelData[8];
    short voiceAddress[8], registerAddress[8] = {0, 0, 0, 0, 0, 0, 1, 0};

    voice = numberVoicesLimiter(voice);

    decimalBinArray(levelRate);
    copyArrays(tempData, levelData, 0, 8);
    levelData[6] = 0;
    levelData[7] = 0;
    copyArrays(levelData, tempData, 0, 8);

    addressCalculator(voice, Operator);
    copyArrays(tempData, voiceAddress,0,8);
    addArrays(voiceAddress, registerAddress);
    send(registerAddress,levelData);
}

void YM3812::attackDecay(short voice, short Operator, short attackRate, short decayRate){
    short attackData[8], decayData[8], attackDecayData[8];
    short voiceAddress[8], registerAddress[8] = {0, 0, 0, 0, 0, 1, 1, 0};

    voice = numberVoicesLimiter(voice);

    decimalBinArray(attackRate);
    copyArrays(tempData, attackData, 0, 4);
    decimalBinArray(decayRate);
    copyArrays(tempData, decayData, 0, 4);

    for(short i = 0; i <= 3; i++){
        attackDecayData[i] = decayData[i];
    }
    for(short i = 4; i <= 7; i++){
        attackDecayData[i] = attackData[i-4];
    }

    addressCalculator(voice, Operator);
    copyArrays(tempData, voiceAddress,0,8);
    addArrays(voiceAddress,registerAddress);

    send(registerAddress,attackDecayData);
}

void YM3812::sustainRelease(short voice, short Operator, short sustainRate, short releaseRate){
    short sustainData[8], releaseData[8], sustainReleaseData[8];
    short voiceAddress[8], registerAddress[8] = {0, 0, 0, 0, 0, 0, 0, 1};

    voice = numberVoicesLimiter(voice);

    decimalBinArray(sustainRate);
    copyArrays(tempData, sustainData, 0, 4);
    decimalBinArray(releaseRate);
    copyArrays(tempData, releaseData, 0, 4);

    for(short i = 0; i <= 3; i++){
        sustainReleaseData[i] = releaseData[i];
    }
    for(short i = 4; i <= 7; i++){
        sustainReleaseData[i] = sustainData[i-4];
    }

    addressCalculator(voice, Operator);
    copyArrays(tempData, voiceAddress,0,8);
    addArrays(voiceAddress, registerAddress);

    send(registerAddress, sustainReleaseData);
}

void YM3812::FM_Vibrato_AM(short voice, short Operator, short modulatorFrequency, bool vibrato, bool ampModulation, bool percusive){
    short CA_address[8] = {0, 0, 1, 0, 1, 0, 0, 0}, voiceAddress[8] = {0,0,0,0,0,0,0,0};
    short CA_data[8] = {0,0,0,0,0,0,0,0};

    addressCalculator(voice, Operator);
    copyArrays(tempData,voiceAddress,0,8);
    addArrays(voiceAddress, CA_address); //combines the address of voices and register

    decimalBinArray(modulatorFrequency);
    copyArrays(tempData, CA_data, 0,4);

    if(percusive)
        CA_data[5] = 1;
    else
        CA_data[5] = 0;

    if(vibrato)
        CA_data[6] = 1;
    else
        CA_data[6] = 0;

    if(ampModulation)
        CA_data[7] = 1;
    else
        CA_data[7] = 0;

    send(CA_address,CA_data);
}

void YM3812::enableRhythmic(){
    short R_Address[8] = {1,0,1,1,1,1,0,1};
    short R_Data[8] = {0,0,0,0,0,5,0,0};
    send(R_Address, R_Data);
}


//Voice playing
void YM3812::playVoice(short voice, short freqData[], short octave, String state){
    short lowBitsData[8] = {0,0,0,0,0,0,0,0}, highBitsData[8] = {0,0,0,0,0,0,0,0};
    short lowBitsAddress[8] = {0,0,0,0,0,1,0,1}, highBitsAddress[8] = {0,0,0,0,1,1,0,1};

    copyArrays(freqData,lowBitsData,0,8); //convert 10bits array to 2 array of 8 bits
    for(byte i=0; i<= 1; i++){
        highBitsData[i] = freqData[i+7];
    }

    decimalBinArray(octave); //convert octave number to array of 8 bits, and copy this array to highBitsData
    for(byte i=0; i <= 2; i++){
        highBitsData[i+2] = tempData[i];
    }

    if(state == "ENABLE")
        highBitsData[5] = 1; //Key ON
    else
        highBitsData[5] = 0; //Key OFF

    decimalBinArray(voice);
    copyArrays(tempData,lowBitsAddress,0,4);
    copyArrays(tempData,highBitsAddress,0,4);

    send(lowBitsAddress,lowBitsData);
    send(highBitsAddress,highBitsData);
}

void YM3812::playRhythmic(bool bassDrum, bool snareDrum, bool tomTom, bool cymbal, bool hiHat){
    short drumData[8] = {0,0,0,0,0,5,0,0};
    short drumAddress[8] = {1,0,1,1,1,1,0,1};

    if(bassDrum)
        drumData[4] = 1;
    else
        drumData[4] = 0;

    if(snareDrum)
        drumData[3] = 1;
    else
        drumData[3] = 0;

    if(tomTom)
        drumData[2] = 1;
    else
        drumData[2] = 0;

    if(cymbal)
        drumData[1] = 1;
    else
        drumData[1] = 0;

    if(hiHat)
        drumData[0] = 1;
    else
        drumData[0] = 0;

    send(drumAddress,drumData);
}