const int sensorPin1 = A4;
const int sensorPin2 = A5;
const int button = 2;
int Sensor1 = 0;
int Sensor2 = 0;
int threshold = 700;
int motorTurnA = 12;
int motorSpeedA = 3;
int motorTurnB = 13;
int motorSpeedB = 11;
int speedForward = 50;
int speedTurn = 100;
int buttonState;
int trigPin = 10;
int echoPin = A3;
long duration, cm, inches;
int e = 0;
int d = 0;
void setup() {

  Serial.begin(9600);

  //Opsætning af porte for afstandmåler
  pinMode(sensorPin1, INPUT);
  pinMode(sensorPin2, INPUT);
  pinMode(trigPin, OUTPUT);                         //Den digitale pin SAfstandTrigger sættes til output (lyd output)
  pinMode(echoPin, INPUT);

  //Den digitale pin SAstandEkko sættes til input (lyd input)

  //Opsætning af porte for motorer
  pinMode(motorTurnA, OUTPUT);                              // Erklærer at motorTurn er output
  pinMode(motorSpeedA, OUTPUT);                             // Erklærer at motorSpeed er output
  pinMode(motorTurnB, OUTPUT);                              // Erklærer at motorTurn er output
  pinMode(motorSpeedB, OUTPUT);                             // Erklærer at motorSpeed er output

  //opsætning af porte for buttons
  pinMode(button, INPUT_PULLUP);
}

void loop() {

   // The sensor is triggered by a HIGH pulse of 10 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the signal from the sensor: a HIGH pulse whose
  // duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(echoPin, INPUT);
  duration = pulseIn(echoPin, HIGH);

  // convert the time into a distance
  cm = (duration/2) / 29.1;
  inches = (duration/2) / 74;


  //Aflæser input for lyssensor
  Sensor1 = analogRead(sensorPin1);
  Sensor2 = analogRead(sensorPin2);

  //Printer lyssensor input
//  Serial.print(Sensor2); // venstre
//  Serial.print(", ");
//  Serial.print(Sensor1); // højre
//  Serial.print(", ");

  if (Sensor1<threshold && Sensor2>threshold) {             // Sensor1 ser hvid og Sensor2 (venstre) ser sort - drej til venstre
    digitalWrite(motorTurnA, HIGH);
    analogWrite(motorSpeedA, speedForward);
    digitalWrite(motorTurnB, LOW);
    analogWrite(motorSpeedB, speedTurn);
    }
  else if (Sensor1>threshold && Sensor2<threshold) {        // Sensor2 ser hvid og Sensor1 (højre) ser sort - drej til højre
    digitalWrite(motorTurnA, LOW);
    analogWrite(motorSpeedA, speedTurn);
    digitalWrite(motorTurnB, HIGH);
    analogWrite(motorSpeedB, speedForward);
    }


  else{
    digitalWrite(motorTurnA, HIGH);
  //  analogWrite(motorSpeedA, speedForward);
    digitalWrite(motorTurnB, HIGH);
  //  analogWrite(motorSpeedB, speedForward);
    }


  //Serial.print(",");
  Serial.println(cm);
  Serial.print(" CM: ");
  //Serial.println(e);
  //Serial.print(buttonState);

  if (cm<10) {
    e++;
  Serial.print(d);
//  Serial.print(" D: ");
//  Serial.print(" E: ");
  Serial.print(e);
    while (e>3 && d<300)
    {
    digitalWrite(motorTurnA, HIGH);
    analogWrite(motorSpeedA, speedForward);
    digitalWrite(motorTurnB, LOW);
    analogWrite(motorSpeedB, speedTurn);
    d++;
    Serial.println(d);
    }
    while (d>30)
    {
    buttonState = digitalRead(button);
      if (buttonState == HIGH){
    Serial.println("HIGH");
    Serial.println(buttonState);
    digitalWrite(motorTurnA, LOW);
    analogWrite(motorSpeedA, speedForward);
    digitalWrite(motorTurnB, HIGH);
    analogWrite(motorSpeedB, speedTurn);
      }  else {
    Serial.println("LOW");
    Serial.println(buttonState);
    digitalWrite(motorTurnA, HIGH);
    analogWrite(motorSpeedA, speedForward);
    digitalWrite(motorTurnB, LOW);
    analogWrite(motorSpeedB, speedTurn);
      }
      }
    }

   }