int LapNumber = -1;
int LapNumber1 = -1;
int Sensor1 = 0;
int Sensor2 = 0;
int Sensor3 = 0;
int Reset1 = 0;


int MobilA = -1;
int MobilB = -1;
int MobilC = -1;

int TimeMobilA = -1;
int TimeMobilB = -1;
int TimeMobilC = -1;

int WaktuMobilA[5];
int WaktuMobilB[5];
int WaktuMobilC[5];



unsigned long Finish_MobilA;
unsigned long FinishSeconds_MobilA;
unsigned int FinishMS_MobilA;

bool ZeroLap = false;
bool Lap1 = false;
String MobilZeroLap;



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
  Serial.begin(9600);
  Serial.println("------STARTED-----");
}

void loop() {
  // put your main code here, to run repeatedly:
  Sensor1 = digitalRead(7);
  Sensor2 = digitalRead(6);
  Sensor3 = digitalRead(5);
  Reset1 = digitalRead(4);


  if (Reset1 == HIGH)
  {
     LapNumber = -1;
     LapNumber1 = -1;
     MobilA = -1;
     MobilB = -1;
     MobilC = -1;
     ZeroLap = false;
     Lap1 = false;
     MobilZeroLap = "";
     Serial.println("----RESET-----");
     Serial.print("LapNumber: ");
      Serial.print(LapNumber);
      Serial.print("- MobilA: ");
      Serial.print(MobilA);
      Serial.print(" - MobilB: ");
      Serial.print(MobilB);
      Serial.print(" - MobilC: ");
      Serial.println(MobilC);
  }

  if(Sensor1 == HIGH){
    digitalWrite(13, HIGH);
    Serial.println("Sensor 1: up");
  }
  else{
    digitalWrite(13, LOW);
  //  Serial.println("Sensor 1: down");
  }

  if(Sensor2 == HIGH){
    digitalWrite(12, HIGH);
    Serial.println("Sensor 2: up");
  }
  else{
     digitalWrite(12, LOW);
  //  Serial.println("Sensor 2: down");
  }

  if(Sensor3 == HIGH){
    digitalWrite(11, HIGH);
    Serial.println("Sensor 3: up");
  }
  else{
    digitalWrite(11, LOW);
  //  Serial.println("Sensor 3: down");
  }

  //Mulai 
  if (Sensor1 == HIGH || Sensor2 == HIGH || Sensor3 == HIGH)
  {
    if (!ZeroLap)
    { 
        if (Sensor1 == HIGH)
        {
           MobilZeroLap = "MobilA";
           WaktuMobilA[0]=millis();
           //krn Mobil A sudah start maka B & Cdianggap juga sudah start
           WaktuMobilB[0]=WaktuMobilA[0];
           WaktuMobilC[0]=WaktuMobilA[0];
        }
        if (Sensor2 == HIGH)
        {
           MobilZeroLap = "MobilB";
           WaktuMobilB[0]=millis();
           //Karena Mobil B sudah start maka A & C dianggap juga sudah start
           WaktuMobilC[0]=WaktuMobilB[0];
           WaktuMobilA[0]=WaktuMobilB[0];
        }
        if (Sensor3 == HIGH)
        {
          MobilZeroLap = "MobilC";
          WaktuMobilC[0]=millis();
          // Karena mobil C sudah start maka mobil A & B dianggap sudah start
          WaktuMobilA[0]=WaktuMobilC[0];
          WaktuMobilB[0]=WaktuMobilC[0];
        }

      Serial.println("Lap 0");
      ZeroLap=true;
    }

   if (LapNumber1 == 0 && ZeroLap) //Sudah memasuki Lap 1
    {
        Serial.println("Lap 1");
        Lap1 = true;
    }

       Serial.print("ZeroLap: ");
       Serial.print(ZeroLap);
       Serial.print(" - MobilZeroLap: ");
       Serial.print(MobilZeroLap);
       Serial.print(" - LapNumber: ");
       Serial.print(LapNumber);
       Serial.print(" - LapNumber1: ");
       Serial.print(LapNumber1);
       Serial.println();
    LapNumber1 = LapNumber1+1;
    // if (Sensor1 == HIGH)
    // {
    //    MobilA = MobilA + 1;
    // }
    // if (Sensor2 == HIGH)
    // {
    //    MobilB = MobilB + 1;
    // }
    // if (Sensor3 == HIGH)
    // {
    //    MobilC = MobilC + 1;
    // }

    // if (MobilA < 3 || MobilB < 3 || MobilC < 3)
    // {
    //    LapNumber = LapNumber +1;
    //    Serial.print("ZeroLap: ");
    //    Serial.print(ZeroLap);
    //    Serial.print(" - MobilZeroLap: ");
    //    Serial.print(MobilZeroLap);
    //    Serial.print(" - LapNumber: ");
    //    Serial.print(LapNumber);
    //    Serial.print("- MobilA: ");
    //    Serial.print("MobilA: ");
    //    Serial.print(MobilA);
    //    Serial.print(" - MobilB: ");
    //    Serial.print(MobilB);
    //    Serial.print(" - MobilC: ");
    //    Serial.println(MobilC);
       

    // } else    //Kalau sudah finish
    // {
    //    Finish_MobilA=millis()-WaktuMobilA[0];
    //    ms_to_seconds(Finish_MobilA,FinishSeconds_MobilA,FinishMS_MobilA);
      

    //   //tampilkan hasil setelah finish
    //   Serial.print("ZeroLap: ");
    //   Serial.print(ZeroLap);
    //   Serial.print(" - MobilZeroLap: ");
    //   Serial.print(MobilZeroLap);
    //   Serial.println(" - ");
    //   Serial.println("Finish");
    //   Serial.print("LapNumber: ");
    //   Serial.print(LapNumber);
    //   Serial.print("- MobilA: ");
    //   Serial.print("MobilA: ");
    //   Serial.print(MobilA);
    //   Serial.print(" - MobilB: ");
    //   Serial.print(MobilB);
    //   Serial.print(" - MobilC: ");
    //   Serial.println(MobilC);
    //   Serial.print("TimeMobilA: ");
    //   Serial.print(FinishSeconds_MobilA);
    //   Serial.print(":");
    //   Serial.print(FinishMS_MobilA);
    //   Serial.println("***");
    // }

  }  
  
  delay(200); 

}


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