int ds = 1;

int results[] = {0, 0, 0, 0, 0};

int var = 1;
int adirectionPin = 12;
int apwmPin = 3;
int bdirectionPin = 13;
int bpwmPin = 11;
const int SEG_A = 8;
const int SEG_B = 9;
const int SEG_C = 10;
const int SEG_D = A0;
const int SEG_E = A1;
const int SEG_F = A2;
const int SEG_G = A3;

int trig = 1;
int echo = 0;
int duration;
float distance;
long previousTime = 0;

const int DIGIT_ONE_PIN = A4;

const int DIGIT_TWO_PIN = A5;

const byte digits[15] = {

B0111111, // 0
B0000110, // 1
B1011011, // 2
B1001111, // 3
B1100110, // 4
B1101101, // 5
B1111101, // 6
B0000111, // 7
B1111111, // 8
B1101111, // 9
B1111000, // t / 10
B1101101, // s / 11
B0000000, // Turn off all segments
B1110001, // f / 13
};

bool isFinish() {
analogWrite(apwmPin, 0);
analogWrite(bpwmPin, 50);
digitalWrite(adirectionPin, LOW);
digitalWrite(bdirectionPin, LOW);
delay(300);
results[0] = digitalRead(2) == LOW;
results[1] = digitalRead(4) == LOW;
results[2] = digitalRead(5) == LOW;
results[3] = digitalRead(6) == LOW;
results[4] = digitalRead(7) == LOW;
if(results[0] && results[1] && results[2] && results[3] && results[4]){
return true;
}
return false;
}

void setup() {
 pinMode(2, INPUT);
pinMode(4, INPUT);
pinMode(5, INPUT);
pinMode(6, INPUT);
pinMode(7, INPUT);
pinMode(adirectionPin, OUTPUT);
pinMode(apwmPin, OUTPUT);
pinMode(bdirectionPin, OUTPUT);
pinMode(bpwmPin, OUTPUT);

TCCR2B = TCCR2B & B11111000 | B00000111;

pinMode(SEG_A, OUTPUT);
pinMode(SEG_B, OUTPUT);
pinMode(SEG_C, OUTPUT);
pinMode(SEG_D, OUTPUT);
pinMode(SEG_E, OUTPUT);
pinMode(SEG_F, OUTPUT);
pinMode(SEG_G, OUTPUT);
pinMode(DIGIT_ONE_PIN, OUTPUT);
pinMode(DIGIT_TWO_PIN, OUTPUT);

pinMode(trig, OUTPUT);
digitalWrite(trig, LOW);
pinMode(echo, INPUT);

start();
}
long currenttime;
int p = 1;
int i = 9;
int l = 9;
int m = -1;
long int k;
long int j;
int q = 0;
int t = 0;

void start(){
while (p > 0 || l >= 0) {
if (p == 1) {
digitalWrite(DIGIT_ONE_PIN, HIGH);
digitalWrite(DIGIT_TWO_PIN, LOW);
displayDigit(0, DIGIT_ONE_PIN);
delay(5);
digitalWrite(DIGIT_ONE_PIN, LOW);
digitalWrite(DIGIT_TWO_PIN, HIGH);
displayDigit(1, DIGIT_TWO_PIN);
delay(5);
if (millis() > currenttime + 1000) {
currenttime = millis();
p--;
}
}

if (i >= 1 && p == 0) {
digitalWrite(DIGIT_ONE_PIN, HIGH);
digitalWrite(DIGIT_TWO_PIN, LOW);
displayDigit(i, DIGIT_ONE_PIN);
 delay(5);
digitalWrite(DIGIT_ONE_PIN, LOW);
digitalWrite(DIGIT_TWO_PIN, HIGH);
displayDigit(0, DIGIT_TWO_PIN);
delay(5);
if (millis() > currenttime + 1000) {
currenttime = millis();
i--;
l--;
}
}

if (l == 0) {
digitalWrite(DIGIT_ONE_PIN, HIGH);
digitalWrite(DIGIT_TWO_PIN, LOW);
displayDigit(10, DIGIT_ONE_PIN);
delay(5);
digitalWrite(DIGIT_ONE_PIN, LOW);
digitalWrite(DIGIT_TWO_PIN, HIGH);
displayDigit(11, DIGIT_TWO_PIN);
delay(5);
if (millis() > currenttime + 1000) {
currenttime = millis();
l--;
}
}
}
}

void optellen(){
digitalWrite(DIGIT_ONE_PIN, LOW);
digitalWrite(DIGIT_TWO_PIN, HIGH);
displayDigit((millis() - 10000) / 10000 % 10, DIGIT_ONE_PIN);
k = ((millis() - 10000) / 10000 % 10); // Calculate and display the tens digit
delay(3);
digitalWrite(DIGIT_ONE_PIN, HIGH);
digitalWrite(DIGIT_TWO_PIN, LOW);
displayDigit((millis() / 1000) % 10, DIGIT_TWO_PIN);
j = ((millis() / 1000) % 10);
if (j == 10){
j = 0;
}// Calculate and display the ones digit starting from 0
delay(3);
}

void loop(){
while (ds == 1) {
long currentTime = millis();

if(currentTime - previousTime >= 50){

previousTime = millis();
digitalWrite(trig, HIGH);
digitalWrite(trig, LOW);
duration = pulseIn(echo, HIGH);

if(duration>=38000){
// Serial.print("Out range");
}
else{
 distance = duration/58;
// Serial.print(distance);
// Serial.println("cm");
}
}
optellen();

results[0] = digitalRead(2) == LOW;

results[1] = digitalRead(4) == LOW;
results[2] = digitalRead(5) == LOW;
results[3] = digitalRead(6) == LOW;
results[4] = digitalRead(7) == LOW;

switch (var) {
case 5: // finish
if (isFinish() == true) {
analogWrite(apwmPin, 0);
analogWrite(bpwmPin, 0);
ds = 0;
}
break;

case 1: // forward
analogWrite(apwmPin, 67);
analogWrite(bpwmPin, 70);
digitalWrite(adirectionPin, LOW);
digitalWrite(bdirectionPin, LOW);
break;

case 2: // right
if (results[3] || results[4]){
analogWrite(apwmPin, 45);
analogWrite(bpwmPin, 50);
digitalWrite(adirectionPin, HIGH);
digitalWrite(bdirectionPin, LOW);
}
break;

case 3: // left
if (results[0] || results[1]){
if(results[3] || results[4]){
var = 2;
}else{
analogWrite(apwmPin, 50);
analogWrite(bpwmPin, 45);
digitalWrite(adirectionPin, LOW);
digitalWrite(bdirectionPin, HIGH);
}
}
break;

case 4: // u-turn
analogWrite(apwmPin, 70);
analogWrite(bpwmPin, 60);
digitalWrite(adirectionPin, LOW);
digitalWrite(bdirectionPin, HIGH);
break;

case 6: // correctie rechts

 analogWrite(apwmPin, 65);
analogWrite(bpwmPin, 70);
digitalWrite(adirectionPin, LOW);
digitalWrite(bdirectionPin, LOW);
break;

case 7: //correctie links

analogWrite(apwmPin, 70);
analogWrite(bpwmPin, 65);
digitalWrite(adirectionPin, LOW);
digitalWrite(bdirectionPin, LOW);
break;
}

results[0] = digitalRead(2) == LOW;

results[1] = digitalRead(4) == LOW;
results[2] = digitalRead(5) == LOW;
results[3] = digitalRead(6) == LOW;
results[4] = digitalRead(7) == LOW;

if ((distance <= 10) || (!results[0] && !results[1] && !results[2] && !

results[3] && !results[4])){
var = 4;
}
else if (results[0] && results[1] && results[2] && results[3] && results[4]) {
var = 5;
}
else if (!results[0] && !results[1] && results[2] && !results[3] && !
results[4]) {
var = 1;
}
else if (!results[0] && !results[1] && !results[2] && !results[3] && results[4]
|| !results[0] && !results[1] && !results[2] && results[3] && results[4] || !
results[0] && !results[1] && results[2] && results[3] && results[4]){
var = 2;
}
else if (results[0] && !results[1] && !results[2] && !results[3] && !results[4]
|| results[0] && results[1] && !results[2] && !results[3] && !results[4] ||
results[0] && results[1] && results[2] && !results[3] && !results[4]){
var = 3;
}
else if (!results[0] && !results[1] && results[2] && results[3] && !results[4]
|| !results[0] && !results[1] && !results[2] && results[3] && !results[4]){
var = 6;
}
else if(!results[0] && results[1] && results[2] && !results[3] && !results[4]
|| !results[0] && results[1] && !results[2] && !results[3] && !results[4]){
var = 7;
}
}

while (ds == 0 && t < 4){

if (q == 0){
digitalWrite(DIGIT_ONE_PIN, HIGH);
digitalWrite(DIGIT_TWO_PIN, LOW);
displayDigit(j, DIGIT_ONE_PIN);
delay(5);
digitalWrite(DIGIT_ONE_PIN, LOW);
digitalWrite(DIGIT_TWO_PIN, HIGH);
 displayDigit(k, DIGIT_TWO_PIN);
delay(5);
if (millis() > currenttime + 500) {
long currenttime = millis();
q++;
}
}

if (q == 1){
digitalWrite(DIGIT_ONE_PIN, HIGH);
digitalWrite(DIGIT_TWO_PIN, LOW);
displayDigit(12, DIGIT_ONE_PIN);
delay(5);
digitalWrite(DIGIT_ONE_PIN, LOW);
digitalWrite(DIGIT_TWO_PIN, HIGH);
displayDigit(12, DIGIT_TWO_PIN);
delay(5);
if (millis() > currenttime + 1000) {
currenttime = millis();
q--;
t++;
}
}
}

while (t == 4){
digitalWrite(DIGIT_ONE_PIN, HIGH);
digitalWrite(DIGIT_TWO_PIN, LOW);
displayDigit(1, DIGIT_ONE_PIN);
delay(5);
digitalWrite(DIGIT_ONE_PIN, LOW);
digitalWrite(DIGIT_TWO_PIN, HIGH);
displayDigit(13, DIGIT_TWO_PIN);
delay(5);
}
}

void displayDigit(int digit, int digitPin) {

digitalWrite(SEG_A, LOW);
digitalWrite(SEG_B, LOW);
digitalWrite(SEG_C, LOW);
digitalWrite(SEG_D, LOW);
digitalWrite(SEG_E, LOW);
digitalWrite(SEG_F, LOW);
digitalWrite(SEG_G, LOW);

// Turn on segments based on the digit

byte pattern = digits[digit];
digitalWrite(SEG_A, bitRead(pattern, 0));
digitalWrite(SEG_B, bitRead(pattern, 1));
digitalWrite(SEG_C, bitRead(pattern, 2));
digitalWrite(SEG_D, bitRead(pattern, 3));
digitalWrite(SEG_E, bitRead(pattern, 4));
digitalWrite(SEG_F, bitRead(pattern, 5));
digitalWrite(SEG_G, bitRead(pattern, 6));
}