#include <gb/gb.h>
#include <rand.h>
#include <stdio.h>
#include <time.h>

#define PU1 0
#define PU2 1
#define WAV 2
#define NOI 3

#define PU1_CURRENT_STATUS 0
#define PU2_CURRENT_STATUS 5
#define WAV_CURRENT_STATUS 10
#define NOI_CURRENT_STATUS 15

#define PU1_CURRENT_NOTE   1
#define PU2_CURRENT_NOTE   6
#define WAV_CURRENT_NOTE   11
#define NOI_CURRENT_NOTE   16

#define PU1_CURRENT_ENV    2
#define PU2_CURRENT_ENV    7
#define WAV_CURRENT_ENV    12
#define NOI_CURRENT_ENV    17

#define PU1_OLD_NOTE       3
#define PU2_OLD_NOTE       8
#define WAV_OLD_NOTE       13
#define NOI_OLD_NOTE       18

#define PU1_OLD_ENV        4
#define PU2_OLD_ENV        9
#define WAV_OLD_ENV        14
#define NOI_OLD_ENV        19

#define PBWHEEL_CENTER     0x40

#define STATUS_PU1_NOTEOFF 0x80
#define STATUS_PU1_NOTE    0x90
#define STATUS_PU1_PB      0xE0
#define STATUS_PU1_CC      0xB0
#define STATUS_PU1_CC_ENV  0x02
#define STATUS_PU1_CC_WAV  0x01
#define STATUS_PU1_CC_PBR  0x04
#define STATUS_PU1_CC_PAN  0x0A
#define STATUS_PU1_CC_SWP  0x03
#define STATUS_PU1_CC_SUS  0x40

#define STATUS_PU2_NOTEOFF 0x81
#define STATUS_PU2_NOTE    0x91
#define STATUS_PU2_PB      0xE1
#define STATUS_PU2_CC      0xB1
#define STATUS_PU2_CC_ENV  0x02
#define STATUS_PU2_CC_WAV  0x01
#define STATUS_PU2_CC_PBR  0x04
#define STATUS_PU2_CC_PAN  0x0A
#define STATUS_PU2_CC_SUS  0x40

#define STATUS_WAV_NOTEOFF 0x82
#define STATUS_WAV_NOTE    0x92
#define STATUS_WAV_PB      0xE2
#define STATUS_WAV_CC      0xB2
#define STATUS_WAV_CC_WAV  0x01
#define STATUS_WAV_CC_OST  0x02
#define STATUS_WAV_CC_RND  0x03
#define STATUS_WAV_CC_PBR  0x04
#define STATUS_WAV_CC_SWP  0x05
#define STATUS_WAV_CC_PAN  0x0A
#define STATUS_WAV_CC_SUS  0x40

#define STATUS_NOI_NOTEOFF 0x83
#define STATUS_NOI_NOTE    0x93
#define STATUS_NOI_PB      0xE3
#define STATUS_NOI_CC      0xB3
#define STATUS_NOI_CC_ENV  0x02
#define STATUS_NOI_CC_PBR  0x04
#define STATUS_NOI_CC_PAN  0x0A
#define STATUS_NOI_CC_SUS  0x40

#define STATUS_POLY_NOTEOFF 0x84
#define STATUS_POLY_NOTE   0x94
#define STATUS_POLY_PB     0xE4
#define STATUS_POLY_CC     0xB4
#define STATUS_POLY_CC_WAV 0x01
#define STATUS_POLY_CC_ENV 0x02
#define STATUS_POLY_CC_PAN 0x0A
#define STATUS_POLY_CC_SUS 0x40

#define STATUS_ALL_NOTES_OFF 0x7B

#define NR60_REG (*(volatile char*)(0xFF30))
#define NR61_REG (*(volatile char*)(0xFF31))
#define NR62_REG (*(volatile char*)(0xFF32))
#define NR63_REG (*(volatile char*)(0xFF33))
#define NR64_REG (*(volatile char*)(0xFF34))
#define NR65_REG (*(volatile char*)(0xFF35))
#define NR66_REG (*(volatile char*)(0xFF36))
#define NR67_REG (*(volatile char*)(0xFF37))
#define NR68_REG (*(volatile char*)(0xFF38))
#define NR69_REG (*(volatile char*)(0xFF39))
#define NR70_REG (*(volatile char*)(0xFF3A))
#define NR71_REG (*(volatile char*)(0xFF3B))
#define NR72_REG (*(volatile char*)(0xFF3C))
#define NR73_REG (*(volatile char*)(0xFF3D))
#define NR74_REG (*(volatile char*)(0xFF3E))
#define NR75_REG (*(volatile char*)(0xFF3F))

UBYTE statusByte  = 0;
UBYTE addressByte = 0;
UBYTE valueByte   = 0;
UBYTE captureDataType=0;
UBYTE capturedAddress=0;

const UWORD freq[72] = {
  44, 	156,  262,  363,  457,  547,  631,  710,  786,  854,  923,  986,
  1046, 1102, 1155, 1205, 1253, 1297, 1339, 1379, 1417, 1452, 1486, 1517,
  1546, 1575, 1602, 1627, 1650, 1673, 1694, 1714, 1732, 1750, 1767, 1783,
  1798, 1812, 1825, 1837, 1849, 1860, 1871, 1881, 1890, 1899, 1907, 1915,
  1923, 1930, 1936, 1943, 1949, 1954, 1959, 1964, 1969, 1974, 1978, 1982,
  1985, 1988, 1992, 1995, 1998, 2001, 2004, 2006, 2009, 2011, 2013, 2015
};

UBYTE noteStatus[20] = {
     // Synth, note status, current note, current env, next note
	0,0,0,255,0,
	0,0,0,255,0,
	0,0,0,255,0,
	0,0,0,255,0
};

UBYTE noiFreq[72] = {  //128
//0xF7,0xF4,0xE7,0xE5,0xE4,0xD7,0xD6,0xD5,
//0xD4,0xC7,0xC6,0xC5,0xC4,0xB7,0xB6,0xB5,
//0xB4,0xA7,0xA6,0xA5,0xA4,0x97,0x96,0x95,
0x94,0x87,0x86,0x85,0x84,0x77,0x76,0x75,0x74,0x67,0x66,0x65,
0x64,0x57,0x56,0x55,0x54,0x47,0x46,0x45,0x44,0x37,0x36,0x35,
0x34,0x27,0x26,0x25,0x24,0x17,0x16,0x15,0x14,0x07,0x06,0x00,

//0xFF,0xFC,0xEF,0xED,0xEC,0xDF,0xDE,0xDD,
//0xDC,0xCF,0xCE,0xCD,0xCC,0xBF,0xBE,0xBD,
//0xBC,0xAF,0xAE,0xAD,0xAC,0x9F,0x9E,0x9D,
0x9C,0x8F,0x8E,0x8D,0x8C,0x7F,0x7E,0x7D,0x7C,0x6F,0x6E,0x6D,
0x6C,0x5F,0x5E,0x5D,0x5C,0x4F,0x4E,0x4D,0x4C,0x3F,0x3E,0x3D,
0x3C,0x2F,0x2E,0x2D,0x2C,0x1F,0x1E,0x1D,0x1C,0x0F,0x0E,0x08
};

UBYTE pu1Env = 7;
UBYTE pu2Env = 7;
UBYTE noiEnv = 8;
UBYTE noiMode = 0;
UBYTE wavShape= 0;
UBYTE wavOffset=0;

UINT8 polyVoiceSelect;
UBYTE polyNoteState[3];

BOOLEAN pu1Sus = 0;
BOOLEAN pu2Sus = 0;
BOOLEAN noiSus = 0;
BOOLEAN wavSus = 0;
BOOLEAN outputSwitch[4] = {0x01,0x02,0x40,0x80};

UBYTE wavShapeLast=0;
UBYTE wavOffsetLast=0;

UWORD pbWheelIn[4] = {0x40,0x40,0x40,0x40};
UWORD pbWheelInLast[4] = {0x40,0x40,0x40,0x40};
BOOLEAN pbWheelActive[4] = {0x00,0x00,0x00,0x00};
UBYTE pbRange[4] = {2,2,2,12};
UBYTE pbNoteRange[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
UWORD currentFreq;
UBYTE midiData[256] = {
	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
};
UBYTE midiDataReadPosition;
UBYTE midiDataWritePosition;

UINT8 i;
UINT8 x;
UINT8 cnt;
UINT8 pos;
UINT8 lastPadRead;
UBYTE lastPadSerial;
BOOLEAN joyState[8] = {0,0,0,0,0,0,0,0};

UBYTE wavSweepSpeed;
UWORD wavCurrentFreq;
UBYTE wavReg[16];
UBYTE wavData[144] = {

	0x22,0x44,0x66,0x88,
	0xAA,0xCC,0xEE,0xFF,
	0xEE,0xCC,0xAA,0x88,
	0x66,0x44,0x22,0x00,
	
	0xFF,0xEE,0xDD,0xCC,
	0xBB,0xAA,0x99,0x88,
	0x77,0x66,0x55,0x44,
	0x33,0x22,0x11,0x00,

	0x22,0x55,0x77,0xAA,
	0xBB,0xDD,0xEE,0xFF,
	0xEE,0xDD,0xBB,0xAA,
	0x66,0x77,0x44,0x00,

	0x22,0xFF,0x66,0x88,
	0xAA,0xCC,0x00,0xFF,
	0xEE,0x55,0xAA,0x88,
	0x66,0x44,0xFF,0x00,
	
	0xFF,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,

	0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,
	0xFF,0xFF,0x00,0x00,
	
	0xFF,0xFF,0xFF,0xFF,
	0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,

	0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x00,
	0xFF,0xFF,0xFF,0xFF,
	0xFF,0xFF,0xFF,0xFF,
};

UBYTE synthUpdateTimer = 0;
UINT16 current_buffer;
UINT16 last_buffer;
UINT16 update_screen;
UINT16 update_wav;

BOOLEAN kb_pu1_state[72];
BOOLEAN kb_pu2_state[72];
BOOLEAN kb_wav_state[72];
UINT8 kb_pu1_note_count=0;
UINT8 kb_pu2_note_count=0;
UINT8 kb_wav_note_count=0;
UBYTE kb_pu1_note_last;
UBYTE kb_pu2_note_last;
UBYTE kb_wav_note_last;
UBYTE kb_pu1_note_chain[12];
UBYTE kb_pu2_note_chain[12];
UBYTE kb_wav_note_chain[12];

INT8 globalTranspose = 0;

void kbResetState(UBYTE synth)
{
	switch(synth){
		case 0:
			for(x=0;x<72;x++) kb_pu1_state[x]=0; 
			kb_pu1_note_count=0; 
			break;
		case 1:
			for(x=0;x<72;x++) kb_pu2_state[x]=0; 
			kb_pu2_note_count=0; 
			break;
		case 2:
			for(x=0;x<72;x++) kb_wav_state[x]=0; 
			kb_wav_note_count=0; 
			break;
	}
}

void kbKeyTrack(UBYTE status,UBYTE note,UBYTE state)
{
	switch(status) {
		case STATUS_PU1_NOTEOFF:
			state=0;
		case STATUS_PU1_NOTE:
			if(note > 35) {
				note-=36;
				kb_pu1_state[note] = (BOOLEAN) state;
				if(state) {
					kb_pu1_note_last = note;
					if(kb_pu1_note_count < 10) kb_pu1_note_count++;
					kb_pu1_note_chain[kb_pu1_note_count] = note;
				} else if(kb_pu1_note_count && kb_pu2_note_last == note) {
					kb_pu1_note_count--;
				}
			}
			break;
		case STATUS_PU2_NOTEOFF:
			state=0;
		case STATUS_PU2_NOTE:
			if(note > 35) {
				note-=36;
				kb_pu2_state[note] = (BOOLEAN) state;
				if(state) {
					kb_pu2_note_last = note;
					if(kb_pu2_note_count < 10) kb_pu2_note_count++;	
					kb_pu2_note_chain[kb_pu2_note_count] = note;
				} else if(kb_pu2_note_count && kb_pu2_note_last == note) {
					kb_pu2_note_count--;
				}
			}
			break;
		case STATUS_WAV_NOTEOFF:
			state=0;
		case STATUS_WAV_NOTE:
			if(note > 23) {
				note-=24;
				kb_wav_state[note] = (BOOLEAN) state;
				if(state) {
					kb_wav_note_last = note;
					if(kb_wav_note_count < 10) kb_wav_note_count++;
					kb_wav_note_chain[kb_wav_note_count] = note;
				} else if(kb_wav_note_count && kb_wav_note_last == note) {
					kb_wav_note_count--;
				}
			}
			break;
	}
}

void kbResetStates()
{
	kbResetState(0);
	kbResetState(1);
	kbResetState(2);
}

UBYTE seed;

void setWavReg()
{
  NR30_REG = 0x00; // turn WAV off
  
	NR60_REG = wavReg[0];
	NR61_REG = wavReg[1];
	NR62_REG = wavReg[2];
	NR63_REG = wavReg[3];
	NR64_REG = wavReg[4];
	NR65_REG = wavReg[5];
	NR66_REG = wavReg[6];
	NR67_REG = wavReg[7];
	NR68_REG = wavReg[8];
	NR69_REG = wavReg[9];
	NR70_REG = wavReg[10];
	NR71_REG = wavReg[11];
	NR72_REG = wavReg[12];
	NR73_REG = wavReg[13];
	NR74_REG = wavReg[14];
	NR75_REG = wavReg[15];
	
  NR30_REG = 0x80; // turn WAV back on
  NR31_REG = 0xFF; //sound length
  NR32_REG = 0x00; //volume normal (0010000 - 01100000)
  if(noteStatus[WAV_CURRENT_STATUS] || wavSus) NR32_REG = noteStatus[WAV_CURRENT_ENV];
  NR34_REG = 0x80+(0x07&(wavCurrentFreq>>8));
  NR33_REG = wavCurrentFreq;
  pbWheelInLast[WAV] = PBWHEEL_CENTER;
}

void writeRandomWavToRegistry()
{
  seed = DIV_REG;
  seed |= (UWORD)DIV_REG << 8;
  initarand(seed);
  for(x=0;x<16;x++) wavReg[x] = rand();
  setWavReg();
}

void loadWav(UBYTE shape)
{
  wavShapeLast = shape;
  wavOffsetLast = wavOffset;
  i = shape * 16;
  if(!wavOffset || wavOffset%2) {
	for(x=0;x<16;x++) wavReg[x]=wavData[x+i+wavOffset];
  } else {
	for(x=0;x<16;x++) wavData[x+i+wavOffset]<<4|wavData[x+i+1+wavOffset]>>4;
  }
  setWavReg();
}

void setPitchBendFrequencyOffset(UBYTE synth,UBYTE synth_pitch,UBYTE synth_note_range)
{
	if(synth != NOI) {
		if(pbWheelIn[synth] > PBWHEEL_CENTER) {
			currentFreq = freq[noteStatus[synth_pitch]] + ((freq[pbNoteRange[synth_note_range+1]] - freq[noteStatus[synth_pitch]]) * ((((pbWheelIn[synth]) - PBWHEEL_CENTER) * 100) / PBWHEEL_CENTER) / 100);
		} else {
			currentFreq = freq[noteStatus[synth_pitch]] - ((freq[noteStatus[synth_pitch]] - freq[pbNoteRange[synth_note_range]]) * (((PBWHEEL_CENTER - pbWheelIn[synth]) * 100) / PBWHEEL_CENTER) / 100);
		}

		if(synth == PU1) {
			NR14_REG = (currentFreq>>8);
			NR13_REG = currentFreq;
		} else if (synth == PU2) {
			NR24_REG = (currentFreq>>8);
			NR23_REG = currentFreq;
		} else if (synth == WAV) {
			NR34_REG = (currentFreq>>8);
			NR33_REG = currentFreq;
			wavCurrentFreq = currentFreq;
		}
	} else {
		if(pbWheelIn[NOI] > PBWHEEL_CENTER) {
		    noteStatus[NOI_CURRENT_NOTE] = noteStatus[NOI_CURRENT_NOTE];
			currentFreq = noiFreq[noteStatus[NOI_CURRENT_NOTE] + ((pbWheelIn[NOI] - PBWHEEL_CENTER) / 3)];
		} else {
		    noteStatus[NOI_CURRENT_NOTE] = noteStatus[NOI_CURRENT_NOTE];
			currentFreq = noiFreq[noteStatus[NOI_CURRENT_NOTE] - ((PBWHEEL_CENTER - pbWheelIn[NOI])/ 3)];
		}
		NR43_REG = currentFreq;
	}
}

void updatePu1()
{
	if(pbWheelIn[PU1] != PBWHEEL_CENTER) {
		pbWheelActive[PU1] = 1;
		if(pbWheelIn[PU1] != pbWheelInLast[PU1]) {
			pbWheelInLast[PU1] = pbWheelIn[PU1];
			setPitchBendFrequencyOffset(PU1,PU1_CURRENT_NOTE,0);
		}
	} else if(pbWheelActive[PU1] == 1) {
		pbWheelActive[PU1] = 0;
		pbWheelInLast[PU1] = PBWHEEL_CENTER;
		NR14_REG = freq[noteStatus[PU1_CURRENT_NOTE]] >> 8;
		NR13_REG = freq[noteStatus[PU1_CURRENT_NOTE]];
	}
}

void updatePu2()
{
	if(pbWheelIn[PU2] != PBWHEEL_CENTER) {
		pbWheelActive[PU2] = 1;
		if(pbWheelIn[PU2] != pbWheelInLast[PU2]) {
			pbWheelInLast[PU2] = pbWheelIn[PU2];
			setPitchBendFrequencyOffset(PU2,PU2_CURRENT_NOTE,2);
		}
	} else if(pbWheelActive[PU2] == 1) {
		pbWheelActive[PU2] = 0;
		pbWheelInLast[PU2] = PBWHEEL_CENTER;
		NR24_REG = freq[noteStatus[PU2_CURRENT_NOTE]] >> 8;
		NR23_REG = freq[noteStatus[PU2_CURRENT_NOTE]];
	}
}
void updateWav()
{
	if(pbWheelIn[WAV] != PBWHEEL_CENTER) {
		pbWheelActive[WAV] = 1;
		if(pbWheelIn[WAV] != pbWheelInLast[WAV]) {
			pbWheelInLast[WAV] = pbWheelIn[WAV];
			setPitchBendFrequencyOffset(WAV,WAV_CURRENT_NOTE,4);
		}
	} else if(pbWheelActive[WAV] == 1) {
		pbWheelActive[WAV] = 0;
		pbWheelInLast[WAV] = PBWHEEL_CENTER;
		NR34_REG = freq[noteStatus[WAV_CURRENT_NOTE]] >> 8;
		NR33_REG = freq[noteStatus[WAV_CURRENT_NOTE]];
		wavCurrentFreq = freq[noteStatus[WAV_CURRENT_NOTE]];
	}
	if(wavSweepSpeed && wavCurrentFreq && !synthUpdateTimer) {
		wavCurrentFreq-=wavSweepSpeed*16;
		if(wavCurrentFreq>freq[noteStatus[WAV_CURRENT_NOTE]]) wavCurrentFreq=0;
		NR34_REG = wavCurrentFreq >> 8;
		NR33_REG = wavCurrentFreq;
	}
	
	if(wavShapeLast != wavShape || wavOffsetLast != wavOffset) loadWav(wavShape);
}

void updateNoi()
{
	if(pbWheelIn[NOI] != PBWHEEL_CENTER) {
		pbWheelActive[NOI] = 1;
		if(pbWheelIn[NOI] != pbWheelInLast[NOI]) {
			pbWheelInLast[NOI] = pbWheelIn[NOI];
			setPitchBendFrequencyOffset(NOI,NOI_CURRENT_NOTE,6);
		}
	} else if(pbWheelActive[NOI] == 1) {
		pbWheelActive[NOI] = 0;
		pbWheelInLast[NOI] = PBWHEEL_CENTER;
		NR43_REG = noiFreq[noteStatus[NOI_CURRENT_NOTE]];
	}
}

UBYTE pbWheelCombineBytes(UBYTE First, UBYTE Second)
{
	UINT16 _14bit;

	_14bit = (UINT16)Second;
	_14bit<<=7;
	_14bit|=(UINT16)First;

	return (UBYTE) (_14bit / 127);
}

void setOutputPan(UBYTE synth, UBYTE value)
{
  if(value > 96) {
    value = 0x10<<synth;
  } else if (value > 32) {
    value = 0x11<<synth;
  } else {
    value = 0x01<<synth;
  }
  outputSwitch[synth] = value;

  NR51_REG = outputSwitch[0]+outputSwitch[1]+outputSwitch[2]+outputSwitch[3];
}

void playNotePu1(BOOLEAN state, UBYTE note, UBYTE envelope, BOOLEAN polyMode)
{
	if(state) {
		NR12_REG = ((envelope << 1) & 0xF0) + pu1Env; //Take a value from 0 to 127 and make it 0 to 255, and add in the envelope
		NR14_REG = 0x80 + (freq[note]>>8);
		NR13_REG = freq[note];
		
		pbNoteRange[0] = note - pbRange[0];
		pbNoteRange[1] = note + pbRange[0];
	
		noteStatus[PU1_CURRENT_STATUS]=1;
		noteStatus[PU1_CURRENT_NOTE]=note;
	    noteStatus[PU1_CURRENT_ENV]=noteStatus[PU1_OLD_ENV]=envelope;
		pbWheelInLast[PU1] = PBWHEEL_CENTER;
		
	} else {
	    if(note == noteStatus[PU1_CURRENT_NOTE]) {
			noteStatus[PU1_CURRENT_STATUS] = 0;
			if(pu1Sus == 0) {
				NR12_REG = 0x00;
			}
		}
		
		if(!polyMode) {
			if(kb_pu1_note_last == note) {
				while(kb_pu1_note_count && !kb_pu1_state[kb_pu1_note_chain[kb_pu1_note_count]]) {
					kb_pu1_note_count--;
				}
				if(kb_pu1_state[kb_pu1_note_chain[kb_pu1_note_count]]) {
					kb_pu1_note_last = kb_pu1_note_chain[kb_pu1_note_count];
					playNotePu1(1,kb_pu1_note_last,noteStatus[PU1_OLD_ENV],0);
				}
			}
		}
	}
}

void playNotePu2(BOOLEAN state, UBYTE note, UBYTE envelope, BOOLEAN polyMode)
{
	if(state) {
		NR22_REG = ((envelope << 1) & 0xF0) + pu2Env; //Take a value from 0 to 127 and make it 0 to 255, and add in the envelope
		NR24_REG = 0x80 + (freq[note]>>8);
		NR23_REG = (freq[note]);
		
		pbNoteRange[2] = note - pbRange[1];
		pbNoteRange[3] = note + pbRange[1];
		
		noteStatus[PU2_CURRENT_STATUS]=1;
		noteStatus[PU2_CURRENT_NOTE]=note;
	    noteStatus[PU2_OLD_ENV] = noteStatus[PU2_CURRENT_ENV] = envelope;
		pbWheelInLast[PU2] = PBWHEEL_CENTER;

	} else {
		if(note == noteStatus[PU2_CURRENT_NOTE]) {
			noteStatus[PU2_CURRENT_STATUS] = 0;
			if(pu2Sus == 0) {
				NR22_REG = 0x00;
			}
		}
		if(!polyMode) {
			if(kb_pu2_note_last == note) {
				while(kb_pu2_note_count && !kb_pu2_state[kb_pu2_note_chain[kb_pu2_note_count]]) {
					kb_pu2_note_count--;
				}
				
				if(kb_pu2_state[kb_pu2_note_chain[kb_pu2_note_count]]) {
					kb_pu2_note_last = kb_pu2_note_chain[kb_pu2_note_count];
					playNotePu2(1,kb_pu2_note_last,noteStatus[PU2_OLD_ENV],0);
				}
			}
		}
	}
}

void playNoteWav(BOOLEAN state, UBYTE note, UBYTE envelope, BOOLEAN polyMode)
{
	if(state) {
		noteStatus[WAV_OLD_ENV] = envelope;
		envelope = (envelope>>5);
		
		switch(envelope) {
			case 0:
			envelope = 0x00;
			break;
			case 1:
			envelope = 0x60;
			break;
			case 2:
			envelope = 0x40;
			break;
			default:
			envelope = 0x20;
		}
		
		NR32_REG = envelope;
		NR34_REG = 0x07&(freq[note]>>8);
		NR33_REG = (freq[note]);
		wavCurrentFreq = freq[note];
		pbNoteRange[4] = note - pbRange[2];
		pbNoteRange[5] = note + pbRange[2];
		
		noteStatus[WAV_CURRENT_STATUS]=1;
		noteStatus[WAV_CURRENT_NOTE]=note;
	    noteStatus[WAV_CURRENT_ENV] =envelope;
		pbWheelInLast[WAV] = PBWHEEL_CENTER;
		
	} else {
		if(note == noteStatus[WAV_CURRENT_NOTE]) {
			noteStatus[WAV_CURRENT_STATUS] = 0;
			if(wavSus == 0) {
				NR32_REG = 0;
			}
		}
		if(!polyMode) {
			if(kb_wav_note_last == note) {
				while(kb_wav_note_count && !kb_wav_state[kb_wav_note_chain[kb_wav_note_count]]) {
					kb_wav_note_count--;
				}
				
				if(kb_wav_state[kb_wav_note_chain[kb_wav_note_count]]) {
					kb_wav_note_last = kb_wav_note_chain[kb_wav_note_count];
					playNoteWav(1,kb_wav_note_last,noteStatus[WAV_OLD_ENV],0);
				}
			}
		}
	}
}

void playNoteNoi(BOOLEAN state, UBYTE note, UBYTE envelope)
{
	if(state) {
		NR42_REG = ((envelope << 1) & 0xF0) + noiEnv; //Take a value from 0 to 127 and make it 0 to 255, and add in the envelope
		NR43_REG = (noiFreq[note]);
		NR41_REG = 0xFF; //sound length
		NR44_REG = 0x80;
		
		noteStatus[NOI_CURRENT_STATUS]=1;
		noteStatus[NOI_CURRENT_NOTE]=note;
	    noteStatus[NOI_CURRENT_ENV] = noteStatus[NOI_OLD_ENV] = envelope;
		pbWheelInLast[NOI] = PBWHEEL_CENTER;
		
	} else if (!state && note == noteStatus[NOI_CURRENT_NOTE]) {
	    //stop current note
		noteStatus[NOI_CURRENT_STATUS] = 0;
		if(noiSus == 0) {
			NR42_REG = 0x00;
		}
	}
}

void eventNote(UBYTE statusByte,UBYTE addressByte,UBYTE valueByte)
{
	switch (statusByte) {
		case STATUS_PU1_NOTEOFF:
			valueByte=0;
		case STATUS_PU1_NOTE:
			if(addressByte>35) { 
				addressByte=(addressByte-36)+globalTranspose;
				playNotePu1(valueByte,addressByte,valueByte,0);
			}
			break;
		case STATUS_PU2_NOTEOFF:
			valueByte=0;
		case STATUS_PU2_NOTE:
			if(addressByte>35) { 
				addressByte=(addressByte-36)+globalTranspose;
				playNotePu2(valueByte,addressByte,valueByte,0);
			}
			break;
		case STATUS_WAV_NOTEOFF:
			valueByte=0;
		case STATUS_WAV_NOTE:
			if(addressByte>23) { 
				addressByte=(addressByte-24)+globalTranspose;
				playNoteWav(valueByte,addressByte,valueByte,0);
			}
			break;
		case STATUS_NOI_NOTEOFF:
			valueByte=0;
		case STATUS_NOI_NOTE:
			if(addressByte>23) { 
				addressByte-=24;
				playNoteNoi(valueByte,addressByte,valueByte);
			}
			break;
		case STATUS_POLY_NOTEOFF:
			valueByte=0;
		case STATUS_POLY_NOTE:
			if(valueByte) {
				polyVoiceSelect++;
				polyVoiceSelect%=3;
				switch(polyVoiceSelect) {
					case 0:
						polyNoteState[PU1] = addressByte;
						playNotePu1(1,(addressByte-36)+globalTranspose,valueByte,1);
						break;
					case 1:
						polyNoteState[PU2] = addressByte;
						playNotePu2(1,(addressByte-36)+globalTranspose,valueByte,1);
						break;
					case 2:
						polyNoteState[WAV] = addressByte;
						playNoteWav(1,(addressByte-24)+globalTranspose,valueByte,1);
						break;
				}
			} else {
				if(addressByte == polyNoteState[PU1]) {
					playNotePu1(0,addressByte - 36 + globalTranspose,valueByte,1);
				}
				if (addressByte == polyNoteState[PU2]) {
					playNotePu2(0,addressByte - 36 + globalTranspose,valueByte,1);
				}
				if (addressByte == polyNoteState[WAV]) {
					playNoteWav(0,addressByte - 24 + globalTranspose,valueByte,1);
				}
			}
			break;
	}
}

void eventCC(UBYTE statusByte,UBYTE addressByte,UBYTE valueByte)
{
	switch (statusByte) {
		case STATUS_PU1_CC:
			switch(addressByte){
				case STATUS_PU1_CC_WAV:
					NR11_REG = (valueByte >> 5) << 3 << 3 |7; //Take a valueu from 0 to 127 and make it 0 to 3, and add in the sound length
					break;
				case STATUS_PU1_CC_ENV:
					pu1Env = valueByte >> 3;
					break;
				case STATUS_PU1_CC_SWP:
					NR10_REG = valueByte;
					break;
				case STATUS_PU1_CC_PAN:
					setOutputPan(PU1,valueByte);
					break;
				case STATUS_PU1_CC_SUS:
					if(valueByte > 63) {
						pu1Sus = 1;
					} else {
						pu1Sus = 0;
						if(noteStatus[PU1_CURRENT_STATUS] == 0) NR12_REG = 0;
					}
					break;
				case STATUS_PU1_CC_PBR:
					pbRange[0] = valueByte;
					break;
				case STATUS_ALL_NOTES_OFF:
					NR12_REG = 0;
					pu1Sus = 0;
					kbResetState(0);
					noteStatus[PU1_CURRENT_STATUS]=0;
					noteStatus[PU1_CURRENT_NOTE]=0;
					noteStatus[PU1_CURRENT_ENV]=0;
					break;
			}
			break;
		case STATUS_PU2_CC:
			switch(addressByte){
				case STATUS_PU2_CC_WAV:
					NR21_REG = (valueByte >> 5) << 3 << 3 |7; //Take a valueu from 0 to 127 and make it 0 to 3, and add in the sound length
					break;
				case STATUS_PU2_CC_ENV:
					pu2Env = valueByte >> 3;
					break;
				case STATUS_PU2_CC_PAN:
					setOutputPan(PU2,valueByte);
					break;
				case STATUS_PU2_CC_SUS:
					if(valueByte > 63) {
						pu2Sus = 1;
					} else {
						pu2Sus = 0;
						if(noteStatus[PU2_CURRENT_STATUS] == 0) NR22_REG = 0;
					}
					break;
				case STATUS_PU2_CC_PBR:
					pbRange[1] = valueByte;
					break;
				case STATUS_ALL_NOTES_OFF:
					NR22_REG = 0;
					pu2Sus = 0;
					kbResetState(1);
					noteStatus[PU2_CURRENT_STATUS]=0;
					noteStatus[PU2_CURRENT_NOTE]=0;
					noteStatus[PU2_CURRENT_ENV]=0;
					break;
			}
			break;
		case STATUS_WAV_CC:
			switch(addressByte) {
				case STATUS_WAV_CC_WAV:
					wavShape = valueByte >> 4;
					break;
				case STATUS_WAV_CC_OST:
					wavOffset = valueByte >> 2;
					break;
				case STATUS_WAV_CC_PAN:
					setOutputPan(WAV,valueByte);
					break;
				case STATUS_WAV_CC_SUS:
					if(valueByte > 63) {
						wavSus = 1;
					} else {
						wavSus = 0;
						if(noteStatus[WAV_CURRENT_STATUS] == 0) NR32_REG = 0;
					}
					break;
				case STATUS_WAV_CC_PBR:
					pbRange[2] = valueByte;
					break;
				case STATUS_WAV_CC_SWP:
					wavSweepSpeed = valueByte;
					break;
				case STATUS_WAV_CC_RND:
					writeRandomWavToRegistry();
					break;
				case STATUS_ALL_NOTES_OFF:
					NR32_REG = 0;
					kbResetState(2);
					wavSus = 0;
					noteStatus[WAV_CURRENT_STATUS]=0;
					noteStatus[WAV_CURRENT_NOTE]=0;
					noteStatus[WAV_CURRENT_ENV]=0;
					break;
			}
			break;
		case STATUS_NOI_CC:
			switch(addressByte) {
				case STATUS_NOI_CC_ENV:
					noiEnv = valueByte >> 3;
					break;
				case STATUS_NOI_CC_PAN:
					setOutputPan(NOI,valueByte);
					break;
				case STATUS_NOI_CC_SUS:
					if(valueByte > 63) {
						noiSus = 1;
					} else {
						noiSus = 0;
						if(noteStatus[NOI_CURRENT_STATUS] == 0) NR42_REG = 0;
					}
					break;
				case STATUS_ALL_NOTES_OFF:
					NR42_REG = 0;
					noiSus = 0;
					noteStatus[NOI_CURRENT_STATUS]=0;
					noteStatus[NOI_CURRENT_NOTE]=0;
					noteStatus[NOI_CURRENT_ENV]=0;
					break;
			}
			break;
		case STATUS_POLY_CC:
			switch(addressByte) {
				case STATUS_POLY_CC_WAV:
					NR11_REG = NR21_REG = (valueByte >> 5) << 3 << 3 |7; //Take a valueu from 0 to 127 and make it 0 to 3, and add in the sound length
					wavShape = valueByte >> 4;
					break;
				case STATUS_POLY_CC_ENV:
					pu1Env = pu2Env = valueByte >> 3;
					break;
				case STATUS_POLY_CC_PAN:
					setOutputPan(PU1,valueByte);
					setOutputPan(PU2,valueByte);
					setOutputPan(WAV,valueByte);
					break;
				case STATUS_POLY_CC_SUS:
					if(valueByte > 63) {
						pu1Sus = pu2Sus = wavSus = 1;
					} else {
						pu1Sus = pu2Sus = wavSus = 0;
						if(noteStatus[PU1_CURRENT_STATUS] == 0) NR12_REG = 0;
						if(noteStatus[PU2_CURRENT_STATUS] == 0) NR22_REG = 0;
						if(noteStatus[WAV_CURRENT_STATUS] == 0) NR32_REG = 0;
					}
					break;
				case STATUS_ALL_NOTES_OFF:
					NR12_REG = 0;
					NR22_REG = 0;
					NR32_REG = 0;
					kbResetStates();
					pu1Sus = pu2Sus = wavSus = 0;
					noteStatus[PU1_CURRENT_STATUS]=noteStatus[PU1_CURRENT_NOTE]=noteStatus[PU1_CURRENT_ENV]=noteStatus[PU2_CURRENT_STATUS]=noteStatus[PU2_CURRENT_NOTE]=0;
					noteStatus[PU2_CURRENT_ENV]=noteStatus[WAV_CURRENT_STATUS]=noteStatus[WAV_CURRENT_NOTE]=noteStatus[WAV_CURRENT_ENV]=0;
					break;
			}
			break;
		case STATUS_PU1_PB:
			pbWheelIn[0] = pbWheelCombineBytes(addressByte,valueByte);
			break;
		case STATUS_PU2_PB:
			pbWheelIn[1] = pbWheelCombineBytes(addressByte,valueByte);
			break;
		case STATUS_WAV_PB:
			pbWheelIn[2] = pbWheelCombineBytes(addressByte,valueByte);
			break;
		case STATUS_NOI_PB:
			pbWheelIn[3] = pbWheelCombineBytes(addressByte,valueByte);
			break;
		case STATUS_POLY_PB:
			pbWheelIn[0] = pbWheelIn[1] = pbWheelIn[2] = pbWheelCombineBytes(addressByte,valueByte);
			break;
	}
}

void updatePortIO()
{
	receive_byte();
	if(_io_in & 0x80) {
		if(_io_in < 0xF0) {
			statusByte = _io_in;
			captureDataType = 1;
		} else {
			statusByte = 0;
			captureDataType = 0;
		}
		capturedAddress = 0;
	} else if(captureDataType) {
		if(!capturedAddress) {
			capturedAddress = 1;
			addressByte = _io_in;
		} else {
			capturedAddress = 0;
			midiData[midiDataWritePosition] = statusByte;
			midiData[midiDataWritePosition+1] = addressByte;
			midiData[midiDataWritePosition+2] = _io_in;
			midiDataWritePosition+=3;
			
			if(midiDataWritePosition > 246) midiDataWritePosition = 0; 
			
			if(statusByte < 0xA0) kbKeyTrack(statusByte,addressByte,_io_in);
		}
	}
}

void setSoundDefaults()
{
  NR52_REG = 0x8F;  //Turn sound on
  NR50_REG = 0x77;  //Turn on Pulses outs
  
  setOutputPan(0, 64);
  setOutputPan(1, 64);
  setOutputPan(2, 64);
  setOutputPan(3, 64);
  
  loadWav(wavShape); //tRIANGLE

  NR44_REG = 0x80;
  NR41_REG = 0x3F; //sound length

}

void updateMidiBuffer()
{
    if(midiDataReadPosition != midiDataWritePosition) {
		if((midiData[midiDataReadPosition] & 0x80)) {
			if(midiData[midiDataReadPosition] < 0xA0) {
				eventNote(midiData[midiDataReadPosition],midiData[midiDataReadPosition+1],midiData[midiDataReadPosition+2]);
			} else {
				eventCC(midiData[midiDataReadPosition],midiData[midiDataReadPosition+1],midiData[midiDataReadPosition+2]);
			}
			midiDataReadPosition+=3;
		} else {
			midiDataReadPosition++;
		}
		if(midiDataReadPosition > 246) midiDataReadPosition = 0;
	}
}

UINT16 getBufferUse()
{
	UINT16 mbuffer = 0;
	if(midiDataReadPosition == midiDataWritePosition) return mbuffer;
	if(midiDataReadPosition > midiDataWritePosition) {
		mbuffer = midiDataWritePosition + (128 - midiDataReadPosition);
	} else {
		mbuffer = midiDataWritePosition - midiDataReadPosition;
	}
	mbuffer = (mbuffer * 100) / 128;
	return mbuffer;
}

void sendPad()
{
	i = joypad();
	if(i != lastPadRead) {
		lastPadRead = i;
		if(i) {
			if((i & J_A) && !joyState[0]) {
				joyState[0] = 1;
				globalTranspose = 0;
				
				return;
			} else if (joyState[0]) {
				joyState[0] = 0;
				return;
			}
			if((i & J_B) && !joyState[1]) {
				joyState[1] = 1;
				return;
			} else if (joyState[1]) {
				joyState[1] = 0;
				return;
			}
			if((i & J_UP) && !joyState[2]) {
				joyState[2] = 1;
				globalTranspose = -12;
				return;
			} else if (joyState[2]) {
				joyState[2] = 0;
				return;
			}
			if((i & J_RIGHT) && !joyState[5]) {
				joyState[5] = 1;
				globalTranspose = 0;
				return;
			} else if (joyState[5]) {
				joyState[5] = 0;
				return;
			}
			if((i & J_DOWN) && !joyState[3]) {
				joyState[3] = 1;
				globalTranspose = 12;
				return;
			} else if (joyState[3]) {
				joyState[3] = 0;
				return;
			}
			if((i & J_LEFT) && !joyState[4]) {
				joyState[4] = 1;
				globalTranspose = 24;
				return;
			} else if (joyState[4]) {
				joyState[4] = 0;
				return;
			}
			if((i & J_SELECT) && !joyState[6]) {
				joyState[6] = 1;
				return;
			} else if (joyState[6]) {
				joyState[6] = 0;
				return;
			}
			if((i & J_START) && !joyState[7]) {
				joyState[7] = 1;
				NR12_REG = 0;
				NR22_REG = 0;
				NR32_REG = 0;
				kbResetStates();
				pu1Sus = pu2Sus = wavSus = 0;
				noteStatus[PU1_CURRENT_STATUS]=noteStatus[PU1_CURRENT_NOTE]=noteStatus[PU1_CURRENT_ENV]=noteStatus[PU2_CURRENT_STATUS]=noteStatus[PU2_CURRENT_NOTE]=0;
				noteStatus[PU2_CURRENT_ENV]=noteStatus[WAV_CURRENT_STATUS]=noteStatus[WAV_CURRENT_NOTE]=noteStatus[WAV_CURRENT_ENV]=0;
				
				globalTranspose = 0;
				return;
			} else if (joyState[7]) {
				joyState[7] = 0;
				return;
			}
		} else {
			for(x=0;x<8;x++) {
				if(joyState[x]){
					joyState[x] = 0;
				}
			}
			return;
		}
	}
}

void pclock()
{
	printf("Time: %d\n",(UINT16) clock());
}

void speedTestSynths()
{
	UINT16 j;
	pclock();
	for(j=0;j<10000;j++){
		_io_in=0x90;
		updatePortIO();
		_io_in=0x40;
		updatePortIO();
		_io_in=0x7F;
		updatePortIO();
	}
	pclock();
}

void testSynths()
{
	wavSweepSpeed = 110;
	playNoteWav(1, 70, 80, 0);
}

void main()
{
    disable_interrupts();
	printf("\n \n \n \n \n \n \n \n \n \n mGB Version: 1.0.1");
	delay(2000);
	DISPLAY_OFF;
	setSoundDefaults();
	add_SIO(updatePortIO);
    enable_interrupts();
	
	//testSynths();
	
	for(;;) {
		updatePu1();
		updatePu2();
		updateWav();
		updateNoi();

		sendPad();
		synthUpdateTimer++;
		if(synthUpdateTimer > 16) synthUpdateTimer=0;
		updateMidiBuffer();
	}
}