#include <WebUSB.h>

#define IN1_1 2
#define IN2_1 3
#define IN3_1 4
#define IN4_1 5

#define IN1_2 10
#define IN2_2 16
#define IN3_2 14
#define IN4_2 15

#define REED_LH2 6
#define REED_LOX2 7
#define REED_RP1 8
#define REED_LOX1 9

String currentCommand = "";
unsigned long prevTime = 0;
boolean unloading[4] = {false, false, false, false};

WebUSB WebUSBSerial(1 /* https:// */, "pole-jeune.pages.clubelek.fr/atelier_igb_apollo/activity_1/");
#define Serial WebUSBSerial

void loadProp(String tank) {
  if (tank == "RP1") {
    digitalWrite(IN1_1, LOW);
    digitalWrite(IN2_1, HIGH);   
    unloading[2] = false;
  } else if (tank == "LH2") {
    digitalWrite(IN3_1, HIGH);
    digitalWrite(IN4_1, LOW);   
    unloading[0] = false;
  } else if (tank == "LOX1") {
    digitalWrite(IN1_2, LOW);
    digitalWrite(IN2_2, HIGH);   
    unloading[3] = false;
  } else if (tank == "LOX2") {
    digitalWrite(IN3_2, HIGH);
    digitalWrite(IN4_2, LOW);   
    unloading[1] = false;
  }
}

void unloadProp(String tank) {
  if (tank == "RP1") {
    digitalWrite(IN1_1, HIGH);
    digitalWrite(IN2_1, LOW);
    unloading[2] = true;
  } else if (tank == "LH2") {
    digitalWrite(IN3_1, LOW);
    digitalWrite(IN4_1, HIGH);
    unloading[0] = true;
  } else if (tank == "LOX1") {
    digitalWrite(IN1_2, HIGH);
    digitalWrite(IN2_2, LOW);   
    unloading[3] = true;
  } else if (tank == "LOX2") {
    digitalWrite(IN3_2, LOW);
    digitalWrite(IN4_2, HIGH);   
    unloading[1] = true;
  }
}

void stopLoading(String tank) {
  if (tank == "RP1") {
    digitalWrite(IN1_1, LOW);
    digitalWrite(IN2_1, LOW);
    unloading[2] = false;
  } else if (tank == "LH2") {
    digitalWrite(IN3_1, LOW);
    digitalWrite(IN4_1, LOW);
    unloading[0] = false;
  } else if (tank == "LOX1") {
    digitalWrite(IN1_2, LOW);
    digitalWrite(IN2_2, LOW);   
    unloading[3] = false;
  } else if (tank == "LOX2") {
    digitalWrite(IN3_2, LOW);
    digitalWrite(IN4_2, LOW);   
    unloading[1] = false;
  }
}

void runCommand(String command) {
  //Décomposition de la commande en ID + arguments
  int curIndex = command.indexOf('_');
  String id = command.substring(0, curIndex);
  Serial.print(id);
  String args[5] = {"0", "0", "0", "0", "0"}; //Max 5 args
  for (int i = 0; i < 5; i++) {
    int nextIndex = command.indexOf('_', curIndex + 1);
    args[i] = command.substring(curIndex + 1, nextIndex);
    Serial.print(args[i]);
    curIndex = nextIndex;
    if (args[i].endsWith(";")) {
      args[i].remove(args[i].indexOf(';'));
      break;
    }
  }
  //Lancement des diverses commandes
  if (id == "LOAD") {
    loadProp(args[0]);
  } else if (id == "UNLOAD") {
    unloadProp(args[0]);
  } else if (id == "STOP") {
    stopLoading(args[0]);
  }
}


void setup() {
  while (!Serial) {}
  Serial.begin(9600);
  Serial.println("Arduino connected and answering");

  pinMode(REED_LH2, INPUT_PULLUP);
  pinMode(REED_LOX2, INPUT_PULLUP);
  pinMode(REED_RP1, INPUT_PULLUP);
  pinMode(REED_LOX1, INPUT_PULLUP);

  pinMode(IN1_1, OUTPUT);
  pinMode(IN2_1, OUTPUT);
  pinMode(IN3_1, OUTPUT);
  pinMode(IN4_1, OUTPUT);
  pinMode(IN1_2, OUTPUT);
  pinMode(IN2_2, OUTPUT);
  pinMode(IN3_2, OUTPUT);
  pinMode(IN4_2, OUTPUT);
  delay(1000);

  loadProp("LH2");
  loadProp("RP1");
  loadProp("LOX1");
  loadProp("LOX2");
  delay(3000);
  stopLoading("LH2");
  stopLoading("RP1");
  stopLoading("LOX1");
  stopLoading("LOX2");
  delay(1000);
  unloadProp("LH2");
  unloadProp("RP1");
  unloadProp("LOX1");
  unloadProp("LOX2");
  delay(6000);
  stopLoading("LH2");
  stopLoading("RP1");
  stopLoading("LOX1");
  stopLoading("LOX2");
  Serial.flush();
}


void loop() {
  

  if (millis() - prevTime >= 1000) {
    Serial.print("REEDS_{\"LH2\":");
    Serial.print(digitalRead(REED_LH2));
    Serial.print(",\"LOX2\":");
    Serial.print(digitalRead(REED_LOX2));
    Serial.print(",\"RP1\":");
    Serial.print(digitalRead(REED_RP1));
    Serial.print(",\"LOX1\":");
    Serial.print(digitalRead(REED_LOX1));
    Serial.println("}");
    prevTime = millis();
  }
  
  if (digitalRead(REED_LH2) && !unloading[0]) {
    stopLoading("LH2");
  }
  if (digitalRead(REED_RP1) && !unloading[2]) {
    stopLoading("RP1");
  }
  /*if (digitalRead(REED_LOX1) && !unloading[3]) {
    stopLoading("LOX1");
  }*/
  if (digitalRead(REED_LOX2) && !unloading[1]) {
    stopLoading("LOX2");
  }
  
  if (Serial) {
    while (Serial.available() > 0) {
      char c = '0';
      while (c != ';') {
        c = Serial.read();
        currentCommand += c;
        delay(1);
      }
      runCommand(currentCommand);
      currentCommand = "";
    }
    Serial.flush();
  }
}