//Initializing Std Pin
int RX_pin = 0;
int TX_pin = 1;
int ResetBt_pin = 2;
int LEDStatus_pin = LED_BUILTIN;
int LEDBlue_pin = 3;
int LEDGreen_pin = 4;
int LEDRed_pin = 5;
int M_State = 0;//Game status variable

//Initializing Module variables
int Vin = 5;//Entrée en Volt vers fils
float RRef = 220;//Resistance de référence (Ohm)
float Acuracy = 0.2;//10% de précision sur mesure ohm (+/-)
const int FilArray[] = {0,1,2,3,4,5};//Initializing analogue Pin
float RArray[6];// raw resitance measured on Analogue pin
int SNLastNum = 1;// Debug : dernier num du SN est 1 = impaire / 0 = Paire

int RArrayCorr[6];// standardized resitance measured on Analogue pin on setup
//int RArrayCorr[6] = {100,0,1000,0,0,5000};//Debug
int AnswerPos = -1; //Position du fil à couper
int RArrayGame[6];//Read of the presence of a resistance on Analogue pin during the game

//int RVide = 0;
//int RYellow = 100;
//int RRed = 330;
//int RBlue = 1000;
//int RBlack = 2000;
//int RWhite = 5000;
const int WireArray [] = {0,100,330,1000,2000,5000};//List of all possible resistance

void setup(){
  Serial.begin(9600);
  
  //Declaring LED pin as output
  pinMode(RX_pin, OUTPUT);
  pinMode(TX_pin, INPUT);
  pinMode(ResetBt_pin, INPUT);
  pinMode(LEDStatus_pin, OUTPUT);
  pinMode(LEDBlue_pin, OUTPUT);
  pinMode(LEDGreen_pin, OUTPUT);
  pinMode(LEDRed_pin, OUTPUT);

  digitalWrite(LEDBlue_pin, HIGH);
  digitalWrite(LEDGreen_pin, HIGH);
  digitalWrite(LEDRed_pin, HIGH);
}

void loop(){
  Serial.print("M_State :");
  Serial.println(M_State);
  if (M_State == 0) {
    //Blink on
    delay(100);
    digitalWrite(LEDBlue_pin, LOW);
    //Reset code
    ReadWire();
    //Blink on
    delay(100);
    digitalWrite(LEDBlue_pin, HIGH);
    //Confirmation appel opérateur
    if (digitalRead(ResetBt_pin) == HIGH) {M_State = 1;}
  }
  if (M_State == 1) {
    //Check code
    digitalWrite(LEDBlue_pin, LOW);
    SolvePuzzle();
    Serial.println(AnswerPos);
    delay(1000);
    digitalWrite(LEDBlue_pin, HIGH);
    if (AnswerPos == -1) {M_State = 0;}
    else {
      for (byte f = 0; f < 6; f++) {
        if (RArrayCorr[f] == 0) {RArrayGame[f] = 0;}
        else {RArrayGame[f] = 1;}
      }
      M_State = 2;
    } 
  }
  if (M_State == 2) {
    //Game code
    //delay(1000);
    digitalWrite(LEDBlue_pin, LOW);
    GameLoop();
    digitalWrite(LEDBlue_pin, HIGH);

  }
  if (M_State == 3) {
      //error Mode
      digitalWrite(LEDRed_pin, LOW);
      digitalWrite(TX_pin, HIGH);
      delay(1000);
      digitalWrite(LEDRed_pin, HIGH);
      M_State = 2;
    }
  if (M_State == 4) {
      //end game
      digitalWrite(LEDGreen_pin, LOW);
      digitalWrite(TX_pin, HIGH);
    }
}

void ReadWire(){
  for (byte f = 0; f < 6; f++) {
    RArray[f] = RRef * ((Vin/((analogRead(f) * Vin)/1024.0)) - 1);
    for (int RCorr : WireArray) { // for each element in the array
      if (RArray[f] <= ((RCorr * (1+Acuracy)) + 10)) {
        if (RArray[f] > ((RCorr * (1-Acuracy)) - 10)){
          RArrayCorr[f] = RCorr;
        }
      }
    } 
  }
  for (byte d = 0; d < 6; d++) {//Debug
  Serial.print(RArray[d]);
  Serial.print(",");
  }
  Serial.println("");
  for (byte d = 0; d < 6; d++) {//Debug
  Serial.print(RArrayCorr[d]);
  Serial.print(",");
  }
  Serial.println("");
}

void SolvePuzzle() {
  //Calc NbFils
  int NbFils = 0;//Nombre de fils connectés
  for (byte f = 0; f < 6; f++) {NbFils += (RArrayCorr[f] != 0);}
  //Serial.println(NbFils);

  //Calc WirePos
  int WirePos[NbFils];//Position de chaque fil occupé
  int p = -1;
  for (byte f = 0; f < 6; f++) {
    if (RArrayCorr[f] != 0) {
      p++;
      WirePos[p] = f;
    }
  } 

  //int RVide = 0;
  //int RYellow = 100;
  //int RRed = 330;
  //int RBlue = 1000;
  //int RBlack = 2000;
  //int RWhite = 5000;

  //Calc Number of each wire
  int LastYellow = -1;//Last Yellow Wire
  int LastRed = -1;//Last Red Wire
  int LastBlue = -1;//Last Blue Wire
  int LastBlack = -1;//Last Black Wire
  int LastWhite = -1;//Last White Wire
  for (byte f = 0; f < NbFils; f++) {
    if (RArrayCorr[WirePos[f]] == 100) {LastYellow++;}//Yellow
    if (RArrayCorr[WirePos[f]] == 330) {LastRed++;}//Red
    if (RArrayCorr[WirePos[f]] == 1000) {LastBlue++;}//Blue
    if (RArrayCorr[WirePos[f]] == 2000) {LastBlack++;}//Black
    if (RArrayCorr[WirePos[f]] == 5000) {LastWhite++;}//White
  } 

  //Calc SNLastNum
  SNLastNum = digitalRead(RX_pin);
  SNLastNum = 1; // Debug : dernier num du SN est 1 = impaire / 0 = Paire
  
  if (NbFils < 3) {
    AnswerPos = -1;
  }
  else if (NbFils == 3){
    //S'il n'y a pas de fil rouge, couper le deuxième fil.
    if (LastRed == -1) {AnswerPos = WirePos[1];}
    //Sinon, si le dernier fil est blanc, couper le dernier fil.
    else if (RArrayCorr[WirePos[NbFils-1]] == 5000) {AnswerPos = WirePos[2];}
    //Sinon, s'il y a plus d'un fil bleu, couper le dernier fil bleu.
    else if (LastBlue > 01) {AnswerPos = WirePos[LastBlue];}
    //Sinon, couper le dernier fil.
    else{AnswerPos = WirePos[2];}
  }

  else if (NbFils == 4){
    //S'il y a plus d'un fil rouge et si le dernier chiffre du numéro de série est impair, couper le dernier fil rouge.
    if (LastRed > 0 && SNLastNum == 1) {AnswerPos = WirePos[LastRed];}
    //Sinon, si le dernier fil est jaune et s'il n'y a pas de fil rouge, couper le premier fil.
    else if (RArrayCorr[WirePos[NbFils-1]] == 100 && LastRed == -1) {AnswerPos = WirePos[0];}
    //Sinon, s'il y a exactement un fil bleu, couper le premier fil.
    else if (LastBlue == 1){AnswerPos = WirePos[0];}
    //Sinon, s'il y a plus d'un fil jaune, couper le dernier fil.
    else if (LastYellow > 0) {AnswerPos = WirePos[3];}
    //Sinon, couper le deuxième fil.
    else {AnswerPos = WirePos[1];}
  }
  
  else if (NbFils == 5){
    //Si le dernier fil est noir et si le dernier chiffre du numéro de série est impair, couper le quatrième fil.
    if (RArrayCorr[WirePos[NbFils-1]] == 2000 && SNLastNum == 1) {AnswerPos = WirePos[3];}
    //Sinon, s'il y a exactement un fil rouge et plus d'un fil jaune, couper le premier fil.
    else if (LastRed == 0 && LastYellow > 0){AnswerPos = WirePos[0];}
    //Sinon, s'il n'y a pas de fil noir, couper le deuxième fil.
    else if (LastBlack == -1){AnswerPos = WirePos[1];}
    //Sinon, couper le premier fil.
    else {AnswerPos = WirePos[0];}
  }
  
  else if (NbFils == 6){
    //S'il n'y a pas de fil jaune et si le dernier chiffre du numéro de série est impair, couper le troisième fil.
    if (LastYellow == -1 && SNLastNum == 1) {AnswerPos = WirePos[2];}
    //Sinon, s'il y a exactement un fil jaune et plus d'un fil blanc, couper le quatrième fil.
    else if (LastYellow == 0 && LastWhite > 0){AnswerPos = WirePos[3];}
    //Sinon, s'il n'y a pas de fil rouge, couper le dernier fil.
    else if (LastRed == -1){AnswerPos = WirePos[5];}
    //Sinon, couper le quatrième fil.
    else {AnswerPos = WirePos[3];}
  }
}

void GameLoop() {
  //Check master status
  
  //Read wire
  ReadWire();
  //Check game status
  for (byte f = 0; f < 6; f++) {
    //Vérification du branchement des cables
    Serial.print(RArrayCorr[f]);
    Serial.print(",");
    Serial.println(RArrayGame[f]);
    if (RArrayCorr[f] == 0 && RArrayGame[f] == 1) {
      //Rupture de cable
      if (AnswerPos == f) {
        Serial.println("C'est le bon fil");
        M_State = 4;
      }
      else {
        Serial.println("C'est le mauvais fil");
        M_State = 3;
        RArrayGame[f] = 0;
      }
    }
  }
  //Serial.println(0);
  
}