#include <Wire.h>
#include <Adafruit_VL6180X.h>

//comment this line for debugging
#define HID
#ifdef HID
// /!\ You need Both and be care full reading the install instruction of the ArduinoXInput_AVR repository
//https://github.com/dmadison/ArduinoXInput_AVR
//https://github.com/dmadison/ArduinoXInput
#include <XInput.h>
#endif

#define BTN_PIN 12
#define UP_PIN 8
#define DOWN_PIN 9
#define LEFT_PIN 11
#define RIGHT_PIN 10

#define MULTIPLEXER 0x70

#define LASER_FLOOR 5 //increase to increate the minimum distance

#define ABS(val) (val > 0 ? val : -val)

//Constante a ne pas changer
#define JOYSTICK_MAX_VALUE 32767

//on sépare les instances de la librarie car je ne lui fais pas confiance pour opérer plusieurs lasers à la fois
Adafruit_VL6180X lasers[3];
bool allLaserReady = true;


bool laser1State = true;
bool laser2State = true;
bool laser3State = true;

void selectI2C(uint8_t i) {
	if (i > 7) return;

	Wire.beginTransmission(MULTIPLEXER);
	Wire.write(1 << i);
	Wire.endTransmission();
}

void setup_laser(uint8_t laser) {
	selectI2C(laser);
	Wire.beginTransmission(0x29);
	if(!Wire.endTransmission()) {
		lasers[laser] = Adafruit_VL6180X();
		lasers[laser].begin();
	}
	else {
		allLaserReady = false;
	#ifndef HID
		Serial.print("Laser ");
		Serial.print(laser);
		Serial.println(" not found");
	#endif
	}
}

void setup() {
	pinMode(UP_PIN, INPUT_PULLUP);
	pinMode(DOWN_PIN, INPUT_PULLUP);
	pinMode(LEFT_PIN, INPUT_PULLUP);
	pinMode(RIGHT_PIN, INPUT_PULLUP);
	pinMode(BTN_PIN, INPUT_PULLUP);

	Wire.begin();

	#ifndef HID
	Serial.begin(9600);
	#else
	XInput.setAutoSend(true);  // Wait for all controls before sending
	XInput.begin();
	#endif


	setup_laser(0);
	setup_laser(1);
	setup_laser(2);

	// si les laser ne sont pas detecter on bloque le systeme en clignotant
	if(!allLaserReady) {
		pinMode(LED_BUILTIN, OUTPUT);
		while(1) {
			digitalWrite(LED_BUILTIN, true);
			delay(1000);
			digitalWrite(LED_BUILTIN, false);
			delay(1000);
		}
	} else {
		digitalWrite(LED_BUILTIN, true);
	}
}

bool lastBtnState = false;
unsigned int lastRead = 0;
int lastxaxis = 0;
int lastyaxis = 0;
void loop() {
	const bool button = digitalRead(BTN_PIN);

	#ifdef HID
	if(button != lastBtnState) {
		XInput.setButton(BUTTON_A, !button);
		lastBtnState = button;
	}

	//get the position from -1 to 1
	int xaxis = !digitalRead(LEFT_PIN) - !digitalRead(RIGHT_PIN);
	if(xaxis != lastxaxis) {
		XInput.setJoystickX(JOY_LEFT, xaxis * JOYSTICK_MAX_VALUE);
		lastxaxis = xaxis;
	}

	//get the position from -1 to 1
	int yaxis = !digitalRead(UP_PIN) - !digitalRead(DOWN_PIN);
	if(yaxis != lastyaxis) {
		XInput.setJoystickY(JOY_LEFT, yaxis * JOYSTICK_MAX_VALUE);
		lastyaxis = yaxis;
	}

	//set the up / down to the right stick
	unsigned int slider = analogRead(A0) / 1023.0 * JOYSTICK_MAX_VALUE;
	if(ABS(slider - lastRead) > 8)
		XInput.setJoystickY(JOY_RIGHT, slider);


	selectI2C(0);
	bool newState = lasers[0].readRange() < LASER_FLOOR;
	if(newState != laser1State) {
		XInput.setButton(BUTTON_B, newState);
		laser1State = newState;
	}

	selectI2C(1);
	newState = lasers[1].readRange() - 15 < LASER_FLOOR;
	if(newState != laser2State) {
		XInput.setButton(BUTTON_Y, newState);
		laser2State = newState;
	}

	selectI2C(2);
	newState = lasers[2].readRange() - 17 < LASER_FLOOR;
	if(newState != laser3State) {
		XInput.setButton(BUTTON_X, newState);
		laser3State = newState;
	}

	#else
	float slider = analogRead(A0) / 1023.0 * 100;
	Serial.print("Slider: ");
	Serial.println(slider);
	Serial.print("btn: ");
	Serial.println(button);

	// o- 13 20 30
	// c- 0 15 16

	selectI2C(0);
	Serial.println("Laser Bas");
	Serial.println(lasers[0].readRange());
	Serial.println(lasers[0].readRange() < LASER_FLOOR);
	selectI2C(1);
	Serial.println("Laser Mid");
	Serial.println(lasers[1].readRange());
	Serial.println(lasers[1].readRange() - 15 < LASER_FLOOR);
	selectI2C(2);
	Serial.println("Laser Haut");
	Serial.println(lasers[2].readRange());
	Serial.println(lasers[2].readRange() - 17 < LASER_FLOOR);
	delay(1000);
	#endif
}