#include <stdio.h>
#include <io.h>
#include <system.h>
#include "altera_avalon_pio_regs.h"
#include "note_frequencies.h"

#define NUMVOICES 5

int main() {
  unsigned char temp = 0;
  unsigned char byte = 0;
  unsigned char data[2] = {0, 0};
  
  unsigned char status = 0;
  unsigned char note = 0;
  unsigned char vel = 0;
  
  unsigned char onVoices[NUMVOICES] = {0};

                            // freq[0] is MIDI note 21
  /*unsigned int freq[128] = {28, 29, 31, 33, 35, 37, 39, 41,
                            44, 46, 49, 52, 55, 58, 61, 65,
                            69, 73, 78, 82, 87, 92, 98, 104,
                            110, 117, 123, 131, 139, 147, 156,
                            165, 175, 185, 196, 208, 220, 233,
                            247, 262, 277, 294, 311, 330, 349,
                            370, 392, 415, 440, 466, 494, 523,
                            554, 587, 622, 659, 698, 740, 784,
                            831, 880, 932, 988, 1047, 1109, 1319,
                            1397, 1480, 1568, 1661, 1760, 1865, 2093,
                            2218, 2349, 2489, 2637, 2793, 2960, 3136,
                            3322, 3520, 3729, 3951, 4186};*/
  
  int numbytes = 0;
  int nextVoice = 0; // index of the next voice to add
  
  // clear any stray bytes that may be lingering in the register
  IOWR_ALTERA_AVALON_PIO_DATA(STATUS_PIO_BASE, 0xf1);
  
  // clear any sounds from the data registers
  IOWR_ALTERA_AVALON_PIO_DATA(NOTE_1_PIO_BASE, 0);
  IOWR_ALTERA_AVALON_PIO_DATA(NOTE_2_PIO_BASE, 0);
  IOWR_ALTERA_AVALON_PIO_DATA(NOTE_3_PIO_BASE, 0);
  IOWR_ALTERA_AVALON_PIO_DATA(NOTE_4_PIO_BASE, 0);
  IOWR_ALTERA_AVALON_PIO_DATA(NOTE_5_PIO_BASE, 0);
  
  //printf("Welcome to the Vocoder!\n");

  while (1) {
    IOWR_ALTERA_AVALON_PIO_DATA(STATUS_PIO_BASE, 0xf0); // disable clearing
    temp = IORD_ALTERA_AVALON_PIO_DATA(STATUS_PIO_BASE); // read status

    if (temp == 1) { // new byte available
        byte = IORD_ALTERA_AVALON_PIO_DATA(DATA_PIO_BASE); // read data reg
        IOWR_ALTERA_AVALON_PIO_DATA(STATUS_PIO_BASE, 0xf1); // clear status reg
        
        if (byte >> 7) { //status signal
            status = byte >> 4; // use only the top nibble (the bottom nibble holds the channel)
            data[0] = 0;
            data[1] = 0;
            numbytes = 0;
        }
        else { // note-on or note-off byte
            data[numbytes] = byte;
            numbytes++;
        }
    }
    
    if (numbytes == 2) {
        note = data[0];
        vel = data[1];
        
        if (status == 0x9) { // note-on event
            if (vel != 0) { // true note-on
                if(((note - 21) >= 0) && ((note - 21) < 85)) { // actual keyboard note
                    //int x = 0;
                    //while (x < NUMVOICES) {
                        //if ((onVoices[x] == 0)) {
                            //onVoices[x] = note;
                            onVoices[nextVoice] = note;
                            // turn note on
                            if( nextVoice == 0 )
                                playNote(note-21, NOTE_1_PIO_BASE);
                            else if( nextVoice == 1 )
                                playNote(note-21, NOTE_2_PIO_BASE);
                            else if( nextVoice == 2 )
                                playNote(note-21, NOTE_3_PIO_BASE);
                            else if( nextVoice == 3 )
                                playNote(note-21, NOTE_4_PIO_BASE);
                            else if( nextVoice == 4 )
                                playNote(note-21, NOTE_5_PIO_BASE);
                            
                            if (nextVoice < 4)
                                nextVoice++;
                            else
                                nextVoice = 0;
                            //printf("Playing MIDI note: %d, Frequency: %d\n", note, freq[note - 21]);
                           // break;
                        //}
                        //x++;
                    //}
                }
            }
            else { // actually a note-off
                if(((note - 21) >= 0) && ((note - 21) < 85)) { // actual keyboard note
                    int x = 0;
                    while (x < NUMVOICES) {
 
                        if (onVoices[x] == note) {
                            onVoices[x] = 0;
                            // turn note off
                            if (x == 0)
                                IOWR_ALTERA_AVALON_PIO_DATA(NOTE_1_PIO_BASE, 0);
                            else if (x == 1)
                                IOWR_ALTERA_AVALON_PIO_DATA(NOTE_2_PIO_BASE, 0);
                            else if (x == 2)
                                IOWR_ALTERA_AVALON_PIO_DATA(NOTE_3_PIO_BASE, 0);
                            else if (x == 3)
                                IOWR_ALTERA_AVALON_PIO_DATA(NOTE_4_PIO_BASE, 0);
                            else if (x == 4)
                                IOWR_ALTERA_AVALON_PIO_DATA(NOTE_5_PIO_BASE, 0);
                            //printf("Stopping MIDI note: %d, Frequency: %d\n", note, freq[note - 21]);
                            break;
                        }
                        x++;
                    }
                }
            }
        }
        else if (status == 0x8) { // explicit note-off event
            if(((note - 21) >= 0) && ((note - 21) < 85)) { // actual keyboard note
                int x = 0;
                while (x < NUMVOICES) {
                    if (onVoices[x] == note) {
                        onVoices[x] = 0;
                        // turn note off
                        if (x == 0)
                            IOWR_ALTERA_AVALON_PIO_DATA(NOTE_1_PIO_BASE, 0);
                        else if (x == 1)
                            IOWR_ALTERA_AVALON_PIO_DATA(NOTE_2_PIO_BASE, 0);
                        else if (x == 2)
                            IOWR_ALTERA_AVALON_PIO_DATA(NOTE_3_PIO_BASE, 0);
                        else if (x == 3)
                            IOWR_ALTERA_AVALON_PIO_DATA(NOTE_4_PIO_BASE, 0);
                        else if (x == 4)
                            IOWR_ALTERA_AVALON_PIO_DATA(NOTE_5_PIO_BASE, 0);
                        //printf("Stopping MIDI note: %d, Frequency: %d\n", note, freq[note - 21]);
                        break;
                    }
                    x++;
                }
            }
        }
        else {
            printf("Here\n");   
        }
        numbytes = 0;
    }
  }
  return 0;
}
