#include <SPI.h>
#define s_prestage 2
#define s_stage 3
#define s_60ft 4
#define s_finish 5
#define b_start 11
#define b_reset 12
#define s_speed 13
#define l_prestage 6
#define l_stage 7
#define l_start1 A4
#define l_start2 A3
#define l_start3 A2
#define l_green A1
#define l_red A0

const unsigned long interval = 500;
unsigned long previousMillis = 0;
int startCount = 0;
float startTimeJS = 0.0;
float currentTimeJS = 0.0;
float timeJS = 0.0;
bool isRunningJS = false;
float startTimeRT = 0.0;
float currentTimeRT = 0.0;
float timeRT = 0.0;
bool isRunningRT = false;
float startTimeET = 0.0;
float currentTimeET = 0.0;
float time60ft = 0.0;
float timeSpeed = 0.0;
float timeET = 0.0;
bool isRunning = false;
const int arrayLampu[7] = {l_red, l_green, l_start3, l_start2, l_start1, l_stage, l_prestage};
bool ft60Logic = true;

void setup() {
  Serial.begin(115200);
  pinMode(s_prestage, INPUT_PULLUP);
  pinMode(s_stage, INPUT_PULLUP);
  pinMode(s_60ft, INPUT_PULLUP);
  pinMode(s_speed, INPUT_PULLUP);
  pinMode(s_finish, INPUT_PULLUP);
  pinMode(b_start, INPUT_PULLUP);
  pinMode(b_reset, INPUT_PULLUP);
  pinMode(l_prestage, OUTPUT);
  pinMode(l_stage, OUTPUT);
  pinMode(l_start1, OUTPUT);
  pinMode(l_start2, OUTPUT);
  pinMode(l_start3, OUTPUT);
  pinMode(l_green, OUTPUT);
  pinMode(l_red, OUTPUT);
  for(byte y=0; y<2; y++){
    for(byte x=0; x<7; x++){
      digitalWrite(arrayLampu[x], LOW);
      delay(50);
    }
    for(byte x=0; x<7; x++){
      digitalWrite(arrayLampu[x], HIGH);
      delay(50);
    }
    delay(500);
  }
  delay(1000);
}

void loop() {
  staging:
  Serial.println("Stanby");
  while(1){
    digitalWrite(l_prestage, digitalRead(s_prestage));
    digitalWrite(l_stage, digitalRead(s_stage));
    if(!digitalRead(b_start)&&!digitalRead(s_prestage)&&!digitalRead(s_stage)){
      goto ready;
    }
  }
  ready:{
    while(1){
      digitalWrite(l_prestage, digitalRead(s_prestage));
      digitalWrite(l_stage, digitalRead(s_stage));
      unsigned long currentMillis = millis();
      if (currentMillis - previousMillis >= interval) {
        previousMillis = currentMillis;
        startCount++;
      }
      if(startCount==1){digitalWrite(l_start1, LOW);digitalWrite(l_start2, HIGH);digitalWrite(l_start3, HIGH);}
      if(startCount==2){digitalWrite(l_start1, HIGH);digitalWrite(l_start2, LOW);digitalWrite(l_start3, HIGH);}
      if(startCount==3){digitalWrite(l_start1, HIGH);digitalWrite(l_start2, HIGH);digitalWrite(l_start3, LOW);}
      if(startCount==4){
        if(!isRunning){
          Serial.println("#,JS,"+String(timeJS, 3)+",#");
          isRunningJS = false;
          startTimeET = millis() / 1000.0;
          isRunning = true;
        }
        if(!digitalRead(s_prestage) && !digitalRead(s_stage) && !isRunningRT){
          startTimeRT = millis() / 1000.0;
          isRunningRT = true;
        }
        digitalWrite(l_start1, HIGH);digitalWrite(l_start2, HIGH);digitalWrite(l_start3, HIGH);digitalWrite(l_green, LOW);
      }

      if(digitalRead(s_prestage) && digitalRead(s_stage) && startCount<4){
        if(!isRunningJS){
          digitalWrite(l_red, LOW);
          startTimeJS = millis() / 1000.0;
          isRunningJS = true;
        }
      }

      if (isRunningJS) {
        currentTimeJS = millis() / 1000.0;
        timeJS = currentTimeJS - startTimeJS;
        Serial.println("Time JS: "+String(timeJS, 3));
      }

      if (isRunningRT) {
        currentTimeRT = millis() / 1000.0;
        timeRT = currentTimeRT - startTimeRT;
        if(startCount>=4){
          Serial.println("Time RT: "+String(timeRT, 3));
        }
      }

      if(digitalRead(s_prestage) && digitalRead(s_stage) && startCount>=4){
        if(isRunningRT){
          Serial.println("#,RT,"+String(timeRT, 3)+",#");
          isRunningRT = false;
        }
      }

      if (isRunning) {
        currentTimeET = millis() / 1000.0;
        timeET = currentTimeET - startTimeET;
        unsigned int seconds = timeET;
        unsigned int millisecs = int(timeET * 1000) % 1000;
        if(startCount>6){
          Serial.println("Time ET: "+String(seconds)+"."+String(millisecs));
        }
      }

      if(startCount > 4){
        if(!digitalRead(s_60ft) && ft60Logic == true){
          time60ft = timeET;
          Serial.println("Time 60ft: "+String(time60ft, 3));
          ft60Logic = false;
        }
        if(!digitalRead(s_speed)){
          timeSpeed = timeET;
        }
        else if(!digitalRead(s_finish)){
          if(isRunning){
            digitalWrite(l_green, HIGH);digitalWrite(l_red, HIGH);
            Serial.println("JS                : "+String(timeJS, 3));
            Serial.println("RT                : "+String(timeRT, 3));
            Serial.println("60ft              : "+String(time60ft, 3));
            Serial.println("Time sensor speed : "+String((timeSpeed + timeRT), 3));
            Serial.println("201m              : "+String(timeET, 3));
            Serial.println("201m + RT         : "+String((timeET + timeRT), 3));
            Serial.println("Waktu tempuh speed: "+String((timeET - timeSpeed), 3));
            Serial.println("Speed             : "+String(((0.001 * 3600)/(timeET - timeSpeed)), 3)+" km/jam");
            isRunning = false;
          }
        }
      }

      if(!digitalRead(b_reset)){
        for(byte x=0; x<7; x++){
          digitalWrite(arrayLampu[x], HIGH);
          delay(50);
        }
        startCount = 0;
        startTimeJS = 0.0;
        currentTimeJS = 0.0;
        timeJS = 0.0;
        isRunningJS = false;
        startTimeRT = 0.0;
        currentTimeRT = 0.0;
        timeRT = 0.0;
        isRunningRT = false;
        startTimeET = 0.0;
        currentTimeET = 0.0;
        time60ft = 0.0;
        timeET = 0.0;
        isRunning = false;
        ft60Logic = true;
        for(byte y=0; y<2; y++){
          for(byte x=0; x<7; x++){
            digitalWrite(arrayLampu[x], LOW);
            delay(50);
          }
          for(byte x=0; x<7; x++){
            digitalWrite(arrayLampu[x], HIGH);
            delay(50);
          }
          delay(500);
        }
        delay(1000);
        goto staging;
      }
    }
  }
}