#include <Wire.h>

#define VS1053_BANK_DEFAULT 0x00
#define GM_ACOUSTIC_PIANO   0x00
#define VELOCITY_FFF        112
#define MIDI_NOTE_ON        0x90
#define MIDI_NOTE_OFF       0x80
#define MIDI_CHAN_MSG       0xB0
#define MIDI_CHAN_BANK      0x00
#define MIDI_CHAN_VOLUME    0x07
#define MIDI_CHAN_PROGRAM   0xC0

#define I2C_CAP1188_A 0x28
#define I2C_CAP1188_B 0x2B
#define CAP1188_RESET_A  -1
#define CAP1188_RESET_B  -1
#define R_SENSOR_THRESHOLD 0x30
#define R_LED_BEHAVIOUR_1  0x81
#define R_LED_BEHAVIOUR_2  0x82
#define R_LED_LINKING      0x72
#define R_SAMPLING_CONFIG  0x24
#define R_SENSITIVITY      0x1F
#define R_GENERAL_CONFIG   0x20
#define R_CONFIGURATION2   0x44
#define R_LED_DIRECT_RAMP  0x94
#define CAP1188_SENINPUTSTATUS 0x3
#define CAP1188_MTBLK 0x2A
#define CAP1188_LEDLINK 0x72
#define CAP1188_PRODID 0xFD
#define CAP1188_MANUID 0xFE
#define CAP1188_STANDBYCFG 0x41
#define CAP1188_REV 0xFF
#define CAP1188_MAIN 0x00
#define CAP1188_MAIN_INT 0x01
#define CAP1188_LEDPOL 0x73

uint8_t keyPrev[128];
uint8_t keyStatus[128];
#define command(Adr,Reg)  I2C_Write_Byte(Adr, Reg,  0x00)
#define data(Adr,Data)    I2C_Write_Byte(Adr, Data, 0x40)
uint8_t octave   = 4;
int8_t  subnote;
#define CHANNEL_0  0

void _delay(uint32_t msec) {
	portTickType delay_ms = msec / portTICK_RATE_MS;
	vTaskDelay(delay_ms);
}

void midiout_write(uint8_t value) {
	Serial1.write(value);
}

void setChannelBank(uint8_t channel, uint8_t bank)
{
	midiout_write(MIDI_CHAN_MSG | channel);
	midiout_write((uint8_t)MIDI_CHAN_BANK);
	midiout_write(bank);
}

void setInstrument(uint8_t channel, uint8_t instrument)
{
	midiout_write(MIDI_CHAN_PROGRAM | channel);  
	midiout_write(instrument);
	_delay(10);
}

void send(uint8_t status, uint8_t data1, uint8_t data2)
{
	midiout_write(status);
	midiout_write(data1);
	midiout_write(data2);
}

void change(uint8_t status, uint8_t data)
{
	midiout_write(status);
	midiout_write(data);
}

void I2C_Write_Byte(uint8_t addr, uint8_t Cmd, uint8_t value)
{
  Wire.beginTransmission(addr);
  Wire.write(Cmd);
  Wire.write(value);
  Wire.endTransmission();
}

uint8_t readRegister(uint8_t addr, uint8_t Cmd)
{
	int val;
	Wire.beginTransmission(addr);
	Wire.write(Cmd);
	Wire.endTransmission();
	Wire.requestFrom(addr, 1);
	while (Wire.available() > 0) {
		val = Wire.read();
	}
	return (uint8_t)val;
}

uint8_t cap1188_touched(uint8_t addr) {
  uint8_t t = readRegister(addr, CAP1188_SENINPUTSTATUS);
  if (t) {
    I2C_Write_Byte(addr, CAP1188_MAIN, readRegister(addr, CAP1188_MAIN) & ~CAP1188_MAIN_INT);
  }
  return t;
}

boolean cap1188_setup(uint8_t addr, int8_t reset) {

	uint8_t pid, mid, rev;
	if (reset >=0 ) {
		pinMode(reset, OUTPUT);
		digitalWrite(reset, LOW);
		_delay(100);
		digitalWrite(reset, HIGH);
		_delay(100);
		digitalWrite(reset, LOW);
		_delay(100);
	}
	// Useful debugging info
	pid = readRegister(addr, CAP1188_PRODID);
	mid = readRegister(addr, CAP1188_MANUID);
	rev = readRegister(addr, CAP1188_REV);

	if ((pid != 0x50) || (mid != 0x5D) || (rev != 0x83)) return false;
	I2C_Write_Byte(addr, CAP1188_MTBLK, 0);
	I2C_Write_Byte(addr, CAP1188_LEDLINK, 0xFF);
	I2C_Write_Byte(addr, CAP1188_STANDBYCFG, 0x30);
	for (int i=0;i<8;i++) I2C_Write_Byte(addr, R_SENSOR_THRESHOLD + i, 0b00000110);
	I2C_Write_Byte(addr, R_LED_BEHAVIOUR_1, 0b00000000);
	I2C_Write_Byte(addr, R_LED_BEHAVIOUR_2, 0b00000000);
	I2C_Write_Byte(addr, R_LED_LINKING,     0b11111111);
	I2C_Write_Byte(addr, R_SAMPLING_CONFIG, 0b00000000);
	I2C_Write_Byte(addr, R_SENSITIVITY,     0b01100000);
	I2C_Write_Byte(addr, R_GENERAL_CONFIG,  0b00111000);
	I2C_Write_Byte(addr, R_CONFIGURATION2,  0b01100000);
	I2C_Write_Byte(addr, R_LED_DIRECT_RAMP, 0);
	memset(keyPrev,0x00,128);
	return true;
}

boolean cap1188_init(TwoWire *wire)
{
	wire->beginTransmission(I2C_CAP1188_A);
	if (wire->endTransmission() != 0) {
		return false;
	} else {
		if (!cap1188_setup(I2C_CAP1188_A, CAP1188_RESET_A)) {
			return false;
		}
	}

	wire->beginTransmission(I2C_CAP1188_B);
	if (wire->endTransmission() != 0) {
		return false;
	} else {
		if (!cap1188_setup(I2C_CAP1188_B, CAP1188_RESET_B)) {
			return false;
		} else {
			return true;
		}
	}
}

void cap1188_keyon(int oct,int subnote)
{
	int noteNo = 12 * (oct + 1) + subnote;
	keyStatus[noteNo] = 1;
}

void cap1188_play()
{
	for (int no=0; no<128; no++) {
		if ((keyPrev[no]==1)&&(keyStatus[no]==0)) {
			keyPrev[no] = 0;
			noteOff(CHANNEL_0|0x80, no, 0);
		} else if ((keyPrev[no]==0)&&(keyStatus[no]==1)) {
			keyPrev[no] = 1;
			noteOn(CHANNEL_0|0x90, no, VELOCITY_FFF);
		}
	}
}

void cap1188_status()
{
	memset(keyStatus, 0x00,128);

	uint8_t touchedA = cap1188_touched(I2C_CAP1188_A);
	uint8_t touchedB = cap1188_touched(I2C_CAP1188_B);
	if (touchedA & 0x01) cap1188_keyon(octave, 0);
	if (touchedA & 0x02) cap1188_keyon(octave, 1);
	if (touchedA & 0x04) cap1188_keyon(octave, 2);
	if (touchedA & 0x08) cap1188_keyon(octave, 3);
	if (touchedA & 0x10) cap1188_keyon(octave, 4);
	if (touchedA & 0x20) cap1188_keyon(octave, 5);
	if (touchedA & 0x40) cap1188_keyon(octave, 6);
	if (touchedA & 0x80) cap1188_keyon(octave, 7);
	if (touchedB & 0x01) cap1188_keyon(octave, 8);
	if (touchedB & 0x02) cap1188_keyon(octave, 9);
	if (touchedB & 0x04) cap1188_keyon(octave,10);
	if (touchedB & 0x08) cap1188_keyon(octave,11);
	if (touchedB & 0x10) cap1188_keyon(octave,12);
	if (touchedB & 0x20) { if (octave > 1 ) octave--;   while(cap1188_touched(I2C_CAP1188_B));};
	if (touchedB & 0x40) { if (octave < 7 ) octave++;   while(cap1188_touched(I2C_CAP1188_B));};
	if (touchedB & 0x80) { while(cap1188_touched(I2C_CAP1188_B));};
	cap1188_play();
}

void setChannelVolume(uint8_t channel, uint8_t vol)
{
	midiout_write(MIDI_CHAN_MSG | channel);
	midiout_write(MIDI_CHAN_VOLUME);
	midiout_write(vol);
}

void noteOn(uint8_t command, uint8_t note, uint8_t velocity)
{
	midiout_write(command);
	midiout_write(note);
	midiout_write(velocity);
	_delay(10);
}

void noteOff(uint8_t command, uint8_t note, uint8_t velocity)
{
	midiout_write(command);
	midiout_write(note);
	midiout_write(velocity);
	_delay(10);
}

void setup()
{
	Serial1.begin(31250);  // midi out
	Wire.begin();
	cap1188_init(&Wire);

	setChannelBank(CHANNEL_0, VS1053_BANK_DEFAULT);
	setChannelVolume(CHANNEL_0, VELOCITY_FFF);
	setInstrument(CHANNEL_0, GM_ACOUSTIC_PIANO);
}

void loop()
{
	cap1188_status();
}