#include <Arduino.h>
// #include <Multiplexer.h>
#include <MIDI_Controller.h>

#define s * 1000
#define TOTAL_LINES 4

/* Pins */
/* - analogs */
unsigned char muxSignal{A1};
/* - digitals */
int muxSelector[TOTAL_LINES]{18, 14, 15, 16};

/* Components */
// Multiplexer m(muxSignal, TOTAL_LINES, muxSelector);
AnalogMultiplex multiplexer(A1, {18, 14, 15, 16});

/* Midi links */
//USBDebugMIDI_Interface midiInterface(115200);

Analog potentiometers[] = {
	{multiplexer.pin(0), MIDI_CC::Channel_Volume, 1},
	{multiplexer.pin(1), MIDI_CC::Channel_Volume, 2},
	{multiplexer.pin(2), MIDI_CC::Channel_Volume, 3},
	{multiplexer.pin(3), MIDI_CC::Channel_Volume, 4},
	{multiplexer.pin(4), MIDI_CC::Channel_Volume, 5},
	{multiplexer.pin(5), MIDI_CC::Channel_Volume, 6},
	{multiplexer.pin(6), MIDI_CC::Channel_Volume, 7},
	{multiplexer.pin(7), MIDI_CC::Channel_Volume, 8}
};

/* Values */
int val[16];
int ref[16];

uint8_t readCapacitivePin(int pinToMeasure);

void setup() {
	Serial.begin(115200);
	while (Serial.available() == 0){
		delay(.5 s);
	}
	// for (size_t i = 0; i < 4; i++) {
		// ref[i] = multiplexer.analogRead(i);
	// }
	ref[8] = readCapacitivePin(2);
	pinMode(0, OUTPUT);
	pinMode(1, OUTPUT);
	pinMode(9, OUTPUT);
}

long loopAverage = 0;
long lastLoop = 0;
bool on = false;

void loop() {
	// long start = millis();
	// for (size_t i = 0; i < 4; i++) {
	// 	val[i] = multiplexer.analogRead(i);
	// 	if (val[i] != ref[i]) {
	// 			Serial.print("p[");
	// 			Serial.print(i, DEC);
	// 			Serial.print("] = ");
	// 			Serial.println(val[i], DEC);
	// 		ref[i] = val[i];
	//
	// 	}
	// }
	val[8] = readCapacitivePin(2);
	if (abs(val[8] - ref[8]) >= 2) {
		if (val[8] > ref[8]) {
			// Serial.print("loop() time : ");
			// Serial.println(lastLoop, DEC);
			// Serial.print("average : ");
			// Serial.println(loopAverage, DEC);
			Serial.println(val[8], DEC);
		}
		ref[8] = val[8];
	}


	// if (val[0] >= 500) {
	// 	digitalWrite(0, HIGH);
	// } else {
	// 	digitalWrite(0, LOW);
	// }
	if (val[2] >= 500) {
		digitalWrite(1, HIGH);
	} else {
		digitalWrite(1, LOW);
	}

	analogWrite(9, val[0] * 256 / 1024);

	MIDI_Controller.refresh();
	// lastLoop = millis() - start;
	// loopAverage = (loopAverage + lastLoop) / 2;
	//delay(.5 s);
}

uint8_t readCapacitivePin(int pinToMeasure) {
	// Variables used to translate from Arduino to AVR pin naming
	volatile uint8_t* port;
	volatile uint8_t* ddr;
	volatile uint8_t* pin;

	// Here we translate the input pin number from
	//  Arduino pin number to the AVR PORT, PIN, DDR,
	//  and which bit of those registers we care about.

	byte bitmask;
	port = portOutputRegister(digitalPinToPort(pinToMeasure));
	ddr = portModeRegister(digitalPinToPort(pinToMeasure));
	bitmask = digitalPinToBitMask(pinToMeasure);
	pin = portInputRegister(digitalPinToPort(pinToMeasure));

	// Discharge the pin first by setting it low and output
	*port &= ~(bitmask);
	*ddr  |= bitmask;
	delay(1);
	uint8_t SREG_old = SREG; //back up the AVR Status Register
	// Prevent the timer IRQ from disturbing our measurement
	noInterrupts();

	// Make the pin an input with the internal pull-up on
	*ddr &= ~(bitmask);
	*port |= bitmask;

	// Now see how long the pin to get pulled up. This manual unrolling of the loop
	// decreases the number of hardware cycles between each read of the pin,
	// thus increasing sensitivity.
	uint8_t cycles = 17;

	if (*pin & bitmask) {
		cycles =  0;
	} else if (*pin & bitmask) {
		cycles =  1;
	} else if (*pin & bitmask) {
		cycles =  2;
	} else if (*pin & bitmask) {
		cycles =  3;
	} else if (*pin & bitmask) {
		cycles =  4;
	} else if (*pin & bitmask) {
		cycles =  5;
	} else if (*pin & bitmask) {
		cycles =  6;
	} else if (*pin & bitmask) {
		cycles =  7;
	} else if (*pin & bitmask) {
		cycles =  8;
	} else if (*pin & bitmask) {
		cycles =  9;
	} else if (*pin & bitmask) {
		cycles = 10;
	} else if (*pin & bitmask) {
		cycles = 11;
	} else if (*pin & bitmask) {
		cycles = 12;
	} else if (*pin & bitmask) {
		cycles = 13;
	} else if (*pin & bitmask) {
		cycles = 14;
	} else if (*pin & bitmask) {
		cycles = 15;
	} else if (*pin & bitmask) {
		cycles = 16;
	}

	// End of timing-critical section; turn interrupts back on if they were on before, or leave them off if they were off before
	SREG = SREG_old;

	// Discharge the pin again by setting it low and output
	//  It's important to leave the pins low if you want to
	//  be able to touch more than 1 sensor at a time - if
	//  the sensor is left pulled high, when you touch
	//  two sensors, your body will transfer the charge between
	//  sensors.
	*port &= ~(bitmask);
	*ddr  |= bitmask;
	return cycles;
}
