#include <18F4525.h>
#device adc=8
#fuses NOWDT,WDT128,HS,NOPROTECT,IESO,BROWNOUT,BORV21,PUT,NOCPD,STVREN,NODEBUG
#fuses NOLVP,NOWRT,NOWRTD,NOEBTR,NOCPB,NOEBTRB,NOWRTC,NOWRTB,FCMEN,NOXINST
#fuses NOPBADEN,NOLPT1OSC,MCLR
#use delay(clock=20000000)

#use rs232(baud=9600,parity=N,xmit=PIN_C6,rcv=PIN_C7,bits=8)

#use fast_io(a)
#use fast_io(b)

#define A0 PIN_A0
#define _RD PIN_A1
#define _WR PIN_A2
#define _CS PIN_A3
#define IRQ PIN_A4

#define DATA 0xFF
#define CONTROL 0x00

void send_cmd(int8, int8);
int8 read_data(int8);
void play_note(int8, int8);
void change_instrument(int8);

void initialize() {

	setup_adc_ports(NO_ANALOGS|VSS_VDD);
	setup_adc(ADC_OFF|ADC_TAD_MUL_0);
	setup_psp(PSP_DISABLED);
	setup_wdt(WDT_OFF);
	setup_timer_0(RTCC_INTERNAL);
	setup_timer_1(T1_DISABLED);
	setup_timer_2(T2_DISABLED,0,1);
	setup_timer_3(T3_DISABLED|T3_DIV_BY_1);
	setup_comparator(NC_NC_NC_NC);
	setup_vref(FALSE);

	// Initialize port directions
	set_tris_a(0b00010000);
	set_tris_b(0b00000000);

	// Initialize signals
	output_low(A0);
	output_high(_RD);
	output_high(_WR);
	output_high(_CS);

	puts("Initialized");
	
	return;
}

void main() {

	initialize();
	
	do {
		// Send command to set to parallel mode
		send_cmd(0x3F, CONTROL);
		delay_us(1);
	// Keep attempting command until we get a
	// response from the synth chip
	} while (!input(IRQ));

	puts("We got data.");
	
	putc(read_data(DATA));		// Expect: 0xFE
	putc(read_data(CONTROL));	// Expect: 0x80
	puts("");
	
	// Change to piano
	change_instrument(0x00);

	while(1) {
		// Play something!
        play_note(0x40, 0x7F);
		puts("Note on");
        delay_ms(5000);
        play_note(0x40, 0x00);
		puts("Note off");

        delay_ms(1000);
	}

	return;

}

void play_note(int8 note_value, int8 velocity) {
	int8 note_cmd;

	send_cmd(0x90, DATA);
	send_cmd(note_value, DATA);
	send_cmd(velocity, DATA);

	return;
}

void change_instrument(int8 instrument) {

	send_cmd(0xB0, DATA);
	send_cmd(0x00, DATA);
	send_cmd(instrument, DATA);
	
	return;
}

int8 read_data(int8 mode) {
	int8 ret_val;

	set_tris_b(0xFF);
	if (mode == CONTROL) {
		output_high(A0);
	} else if (mode == DATA) {
		output_low(A0);
	}
	delay_us(5);
	output_low(_CS);
	delay_us(5);
	output_low(_RD);
	delay_us(5);
	ret_val = input_b();
	output_high(_RD);
	delay_us(5);
	output_high(_CS);
	delay_us(5);

	return (ret_val);
}

void send_cmd(int8 cmd, int8 mode) {
	set_tris_b(0x00);
	if (mode == CONTROL) {
		output_high(A0);
	} else if (mode == DATA) {
		output_low(A0);
	}
	delay_us(5);
	output_low(_CS);
	delay_us(5);
	output_low(_WR);
	delay_us(5);
	output_b(cmd);
	delay_us(5);
	output_high(_WR);
	delay_us(5);
	output_high(_CS);
	delay_us(5);

	return;
}