#define  REG_A_L       0x00   // channel-A 周波数上位4ビットを格納するアドレス
#define  REG_A_H       0x01   // channel-A 周波数下位8ビットを格納するアドレス
#define  REG_B_L       0x02   // channel-B 周波数上位4ビットを格納するアドレス
#define  REG_B_H       0x03   // channel-B 周波数下位8ビットを格納するアドレス
#define  REG_C_L       0x04   // channel-C 周波数上位4ビットを格納するアドレス
#define  REG_C_H       0x05   // channel-C 周波数下位8ビットを格納するアドレス
#define  REG_NOISE     0x06   // ノイズ音の周波数情報(5bit)

#define  REG_MIXER     0x07   // ミキサーの設定情報(6bit)
#define  REG_VOLUME_A  0x08   // channel-Aの音量コントロール情報(5bit)
#define  REG_VOLUME_B  0x09   // channel-Bの音量コントロール情報(5bit)
#define  REG_VOLUME_C  0x0A   // channel-Cの音量コントロール情報(5bit)

#define  REG_ENV_L     0x0B   // エンベロープの周波数情報下位8bit
#define  REG_ENV_H     0x0C   // エンベロープの周波数情報上位8bit
#define  REG_ENV_SHAPE 0x0D   // 	エンベロープ形状(4bit)

#define BASE_FREQ 125000

#define CHANNEL_A        0
#define CHANNEL_B        1
#define CHANNEL_C        2

const byte WRCS_PIN  = 10;
const byte A0_PIN    = 11;
const byte RESET_PIN = 12;

#define VELOCITY_STD     64

static const float NoteFrequency[] = {
	    8.2,     8.7,     9.2,     9.7,    10.3,    10.9,    11.6,    12.2,    13.0,    13.8,
	   14.6,    15.4,    16.4,    17.3,    18.4,    19.4,    20.6,    21.8,    23.1,    24.5,
	   26.0,    27.5,    29.1,    30.9,    32.7,    34.6,    36.7,    38.9,    41.2,    43.7,
	   46.2,    49.0,    51.9,    55.0,    58.3,    61.7,    65.4,    69.3,    73.4,    77.8,
	   82.4,    87.3,    92.5,    98.0,   103.8,   110.0,   116.5,   123.5,   130.8,   138.6,
	  146.8,   155.6,   164.8,   174.6,   185.0,   196.0,   207.7,   220.0,   233.1,   246.9,
	  261.6,   277.2,   293.7,   311.1,   329.6,   349.2,   370.0,   392.0,   415.3,   440.0,
	  466.2,   493.9,   523.3,   554.4,   587.3,   622.3,   659.3,   698.5,   740.0,   784.0,
	  830.6,   880.0,   932.3,   987.8,  1046.5,  1108.7,  1174.7,  1244.5,  1318.5,  1396.9,
	 1480.0,  1568.0,  1661.2,  1760.0,  1864.7,  1975.5,  2093.0,  2217.5,  2349.3,  2489.0,
	 2637.0,  2793.8,  2960.0,  3136.0,  3322.4,  3520.0,  3729.3,  3951.1,  4186.0,  4434.9,
	 4698.6,  4978.0,  5274.0,  5587.7,  5919.9,  6271.9,  6644.9,  7040.0,  7458.6,  7902.1,
	 8372.0,  8869.8,  9397.3,  9956.1, 10548.1, 11175.3, 11839.8, 12543.9,
};

uint8_t busy[3] = {0,0,0};

// 指定したチャンネルの基本となる音量を指定する
void setChannelVolume(uint8_t channel, uint8_t vol)
{
	// Volume:0～127 -> 0～15(4bit)
	uint8_t bit4;
	     if (vol <  10) { bit4 =  0; }
	else if (vol <  22) { bit4 =  1; }
	else if (vol <  34) { bit4 =  3; }
	else if (vol <  46) { bit4 =  5; }
	else if (vol <  58) { bit4 =  7; }
	else if (vol <  70) { bit4 =  8; }
	else if (vol <  82) { bit4 =  9; }
	else if (vol <  94) { bit4 = 11; }
	else if (vol < 106) { bit4 = 13; }
	else                   { bit4 = 15; };
	ymz294_set_register(REG_VOLUME_A + channel, bit4);
}

// 音を鳴らす
void noteOn(uint8_t channel, uint8_t note, uint8_t velocity)
{
	int8_t chno = -1;
	for(uint8_t i=0; i<3; i++) {
		if (busy[i] == note) {
			chno = i;
			break;
		}
	}
	if (chno == -1) {
		for(uint8_t i=0; i<3; i++) {
			if (busy[i] == 0) {
				chno = i;
				busy[i] = note;
				break;
			}
		}
	}
	if (chno == -1) return;

	uint16_t freqbit = 0;
	// 125000/周波数(HZ)の整数値を12ビットで各チャンネルのレジスタにセットする。
	// レジスタは8ビット構成なので、上位4ビットと下位8ビットに分けて格納する。
	// 12ビットなので周波数はB0(No.23/30.9Hz)以上を指定する。
	float freq = NoteFrequency[note];
	if (freq > 0) freqbit = BASE_FREQ / freq;
	freqbit &= 0b0000111111111111; // 12bit
	ymz294_set_register(REG_A_L + (chno * 2), freqbit & 0xff);
	ymz294_set_register(REG_A_H + (chno * 2), (freqbit >> 8) & 0xff);
	setChannelVolume(chno, velocity);
}

// 音を止める
void noteOff(uint8_t channel, uint8_t note, uint8_t velocity)
{
	int8_t chno = -1;
	for(uint8_t i=0; i<3; i++) {
		if (busy[i] == note) {
			chno = i;
			busy[i] = 0;
			break;
		}
	}
	if (chno == -1) return;
	setChannelVolume(chno, 0);
}

void ymz294_reset(void)
{
	// 全てのレジスタに0をセットし、ハードウエアをリセットする
	ymz294_reset_param();
	ymz294_reset_hardware();
}

void ymz294_SetMixer(byte noise, byte square)
{
	uint8_t  mixer;  // Mixer Status (6bit)
	mixer = ((noise & 0b111) << 3) | (square & 0b111);
	ymz294_set_register(REG_MIXER, mixer);
}

void ymz294_reset_param(void)
{
	ymz294_set_register(REG_A_L,   0);
	ymz294_set_register(REG_A_H,   0);
	ymz294_set_register(REG_B_L,   0);
	ymz294_set_register(REG_B_H,   0);
	ymz294_set_register(REG_C_L,   0);
	ymz294_set_register(REG_C_H,   0);
	ymz294_set_register(REG_NOISE, 0);

	ymz294_set_register(REG_MIXER,    0);
	ymz294_set_register(REG_VOLUME_A, 0);
	ymz294_set_register(REG_VOLUME_B, 0);
	ymz294_set_register(REG_VOLUME_C, 0);

	ymz294_set_register(REG_ENV_L,     0);
	ymz294_set_register(REG_ENV_H,     0);
	ymz294_set_register(REG_ENV_SHAPE, 0);
}

void ymz294_reset_hardware(void)
{
	// ハードウエアリセットはWRCS_PINがHIGH、A0_PINがLOWの状態でRESET_PINをLOWにし、
	// 少し間をおいてからRESET_PINをHIGHにする。
	digitalWrite(WRCS_PIN,  HIGH);
	digitalWrite(A0_PIN,    LOW);
	digitalWrite(RESET_PIN, LOW);
	delay(10);
	digitalWrite(RESET_PIN, HIGH);
}

void ymz294_set_register(byte addr, byte value)
{
	// addr
	digitalWrite(WRCS_PIN, LOW);
	digitalWrite(A0_PIN,   LOW);
	PORTD = addr; // PORTD:出力状態設定レジスタ
	// for (int i = 0; i < 8; i++) digitalWrite(DATA_PIN[i], bitRead(addr, i));
	digitalWrite(WRCS_PIN, HIGH); // データ送信

	// value
	digitalWrite(WRCS_PIN, LOW);
	digitalWrite(A0_PIN,   HIGH);
	PORTD = value; // PORTD:出力状態設定レジスタ
	// for (int i = 0; i < 8; i++) digitalWrite(DATA_PIN[i], bitRead(value, i));
	digitalWrite(WRCS_PIN, HIGH); // データ送信
}

void setup()
{
	DDRD = 0b11111111; // DDRD:ポートDの方向レジスタ 0=IN, 1=OUT
	// for (int i = 0; i < 8; i++) pinMode(DATA_PIN[i], OUTPUT);

	pinMode(WRCS_PIN,  OUTPUT);
	pinMode(A0_PIN,    OUTPUT);
	pinMode(RESET_PIN, OUTPUT);

	ymz294_reset();
	ymz294_SetMixer(0b111, 0b000);

	for (int note = 23; note <= 108; note++) {
		noteOn(CHANNEL_A, note, VELOCITY_STD);
		delay(200);
		noteOff(CHANNEL_A, note, 0);
	}

	noteOn(CHANNEL_A, 60, VELOCITY_STD);
	delay(200);
	noteOff(CHANNEL_A, 60, 0);
	noteOn(CHANNEL_B, 64, VELOCITY_STD);
	delay(200);
	noteOff(CHANNEL_B, 64, 0);
	noteOn(CHANNEL_C, 67, VELOCITY_STD);
	delay(200);
	noteOff(CHANNEL_C, 67, 0);

	noteOn(CHANNEL_A, 60, VELOCITY_STD);
	noteOn(CHANNEL_B, 64, VELOCITY_STD);
	noteOn(CHANNEL_C, 67, VELOCITY_STD);
	delay(1000);
	noteOff(CHANNEL_A, 60, 0);
	noteOff(CHANNEL_B, 64, 0);
	noteOff(CHANNEL_C, 67, 0);
}

void loop() {}