unsigned long MobilATime[5];
unsigned long MobilBTime[5];
unsigned long MobilCTime[5];
unsigned long jeda;


String NextSensorMobilA;
String NextSensorMobilB;
String NextSensorMobilC;


int LapNumber = 0;
int MobilALap = -1;
int MobilBLap = -1;
int MobilCLap = -1;


int Sensor1;
int Sensor2;
int Sensor3;

int Sensor1Count = 0;
int Sensor2Count = 0;
int Sensor3Count = 0;



void setup() {
  // put your setup code here, to run once:
  pinMode(13, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(7, INPUT); //sensor 1 kuning
  pinMode(6, INPUT); //sensor 2 merah
  pinMode(5, INPUT); //sensor 3 biru
  pinMode(4, INPUT); //RESET
  pinMode(3, INPUT); //Start button
  Serial.begin(9600);
  Serial.println("------STARTED-----");
  NextSensorMobilA = "Sensor1";
  NextSensorMobilB = "Sensor2";
  NextSensorMobilC = "Sensor3";

  jeda = 0;
}

void loop()
{
  Sensor3 = digitalRead(5);
  Sensor2 = digitalRead(6);
  Sensor1 = digitalRead(7);

  if (Sensor1 == HIGH) {
    // jatahnya mobil A lewat

    if (NextSensorMobilA == "Sensor1") { //mobil a lap 0
      MobilATime[0] = millis();
      MobilALap++;
      NextSensorMobilA = "Sensor2";
      Serial.println("S1 - Mobil A lewat - lap 0");

    }
    if (NextSensorMobilA == "Sensor4") { //mobil a lap 3 - finish
      if (millis()-MobilATime[2] > 3000){
         MobilATime[3] = millis();
         MobilALap++;
         NextSensorMobilA = "NONE";
         Serial.println("S1 - Mobil A lewat - lap 3 - finish");
      }
    }
    if (NextSensorMobilB == "Sensor1") { //mobil b lap 2
      if (millis()-MobilBTime[1]>3000){
        MobilBTime[2] = millis();
        MobilBLap++;
        NextSensorMobilB = "Sensor4";
        Serial.println("S1 - Mobil B lewat - lap 2");
      }
    }
    if (NextSensorMobilC == "Sensor1") { //mobil c lap 1
      if (millis()-MobilCTime[0] > 3000){
         MobilCTime[1] = millis();
         MobilBLap++;
         NextSensorMobilC = "Sensor2";
         Serial.println("S1 - Mobil C lewat - lap 1");
    }
    }

  }
  if (Sensor2 == HIGH) {
    if (NextSensorMobilB == "Sensor2") { //mobil b lap 0
      MobilBTime[0] = millis();
      MobilBLap++;
      NextSensorMobilB = "Sensor3";
      Serial.println("S2 - Mobil B lewat - lap 0");
    }

    if (NextSensorMobilA == "Sensor2") { //mobil a lap 1
      if (millis() - MobilATime[0] > 3000) {
        MobilATime[1] = millis();
        MobilALap++;
        NextSensorMobilA = "Sensor3";
        Serial.println("S2 - Mobil A lewat - lap 1");
      }
    }

    if (NextSensorMobilB == "Sensor4") {
      if (millis()-MobilBTime[2]>3000){
        MobilBTime[3] = millis();
        MobilBLap++;
        NextSensorMobilB = "None";
        Serial.println("S2 - Mobil B lewat - lap 3 - finish");
      }
    }
    if (NextSensorMobilC == "Sensor2") { //mobil c lap 2
      if (millis()-MobilCTime[1] > 3000){
         MobilCTime[2] = millis();
         MobilCLap++;
         NextSensorMobilC = "Sensor4";
         Serial.println("S1 - Mobil C lewat - lap 2");
      }
    }

  }
  if (Sensor3 == HIGH) {
    if (NextSensorMobilC == "Sensor3") { //mobil c lap 0
      MobilCTime[0] = millis();
      MobilCLap++;
      NextSensorMobilC = "Sensor1";
      Serial.println("S3 - Mobil C lewat - lap 0");
    }
    if (NextSensorMobilA == "Sensor3") { //mobil a lap 2
      if(millis()-MobilATime[1] > 3000){
         MobilATime[2] = millis();
        MobilALap++;
        NextSensorMobilA = "Sensor4";
        Serial.println("S3 - Mobil A lewat - lap 2");
      }
    }

    if (NextSensorMobilB == "Sensor3") { //mobil b lap 1
      if (millis() - MobilBTime[0] > 3000) {
        MobilBTime[1] = millis();
        MobilBLap++;
        NextSensorMobilB = "Sensor1";
        Serial.println("S3 - Mobil B lewat - lap 1");
      }
    }
    if (NextSensorMobilC == "Sensor4") { //mobil c lap 3
      if (millis()-MobilCTime[2] > 3000){
         MobilCTime[3] = millis();
         MobilCLap++;
         NextSensorMobilC = "None";
         Serial.println("S1 - Mobil C lewat - lap 3 - finish");
      }
    }
  }
  
  if (MobilALap==3 || MobilBLap==3 || MobilCLap==3) {
   for (int i = 0; i <= 3; i++) {
    Serial.print("Jumlah i: ");
    Serial.print(i);
    Serial.print(" - MobilATime[");
    Serial.print(i);
    Serial.print("] :");
    Serial.print(MobilATime[i]);
    Serial.print(" - MobilBTime[");
    Serial.print(i);
    Serial.print("] :");
    Serial.print(MobilBTime[i]);
    Serial.print(" - MobilCTime[");
    Serial.print(i);
    Serial.print("] :");
    Serial.print(MobilCTime[i]);
    Serial.println("===");
   }
  }  

  delay(200);
}


void ms_to_seconds(unsigned long milliseconds, unsigned long &seconds, unsigned int &remainingMilliseconds) {
  seconds = milliseconds / 1000;
  remainingMilliseconds = milliseconds % 1000;
}