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

//comment this line for debugging
#define HID
#ifdef HID
//https://github.com/MHeironimus/ArduinoJoystickLibrary
#include <Joystick.h>
Joystick_ Joystick(0x03, JOYSTICK_TYPE_GAMEPAD);
#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)

//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
  Joystick.begin();
  Joystick.setXAxisRange(-1, 1);
  Joystick.setYAxisRange(-1, 1);
  Joystick.setRxAxisRange(-1,1);
  #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() {
  // put your main code here, to run repeatedly:
  const bool button = digitalRead(BTN_PIN);

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

  int xaxis = !digitalRead(LEFT_PIN) - !digitalRead(RIGHT_PIN);
  if(xaxis != lastxaxis) {
    Joystick.setXAxis(xaxis);
    lastxaxis = xaxis;
  }

  int yaxis = !digitalRead(UP_PIN) - !digitalRead(DOWN_PIN);
  if(yaxis != lastyaxis) {
    Joystick.setYAxis(yaxis);
    lastyaxis = yaxis;
  }


  //set the up / down to the right stick
  unsigned int slider = analogRead(A0);
  if(ABS(slider - lastRead) > 8)
    Joystick.setRyAxis(slider);


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

  selectI2C(1);
  newState = lasers[1].readRange() - 15 < LASER_FLOOR;
  if(newState != laser2State) {
    //2 & 4 car 2 est censé être le bouton X mais est cable sur 4
    Joystick.setButton(2, newState);
    Joystick.setButton(4, newState);
    laser2State = newState;
  }

  selectI2C(2);
  newState = lasers[2].readRange() - 17 < LASER_FLOOR;
  if(newState != laser3State) {
    Joystick.setButton(3, 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
}