// This code works for the PAMI robots marked with an S and a 1 
// It makes the robot follow the black line and stop in green squares.
// For a second green square stop, tape sensor_2 on the right side, at 3.5cm from 
// the center (0.5cm from the edge). For a first green square stop, tape sensor_2 
// on the left side, at 3.5cm from the center (0.5cm from the edge).

// Ultrasound sensor
const int trigPin = 12; 
const int echoPin = 13;
float duration;
float distance;
unsigned long lastDistanceCheck = 0;


// Motor pins
const int A_Direction = 2; // A = left motor
const int A_Speed = 5;
const int B_Direction = 4; // B = right motor 
const int B_Speed = 6;

const int sensorPin = 8;
const int sensorPin_2 = 9; //side sensor Ligne noire: 1cm width, rectangle noir: 5 cm width 

// Tirette
const int cablePin = 7;

// Timers
//unsigned long thresholdTime =  100; 
bool timerStarted = false;
unsigned long startTime = 0;
bool done = false;
bool isMoving = true; 
const int Black = 1; // 1 = robot to the left of the line , // 0 = right 

// Duration calculation for obstacle deteaction
float distanceUltra() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH) / 58.00; // cm
  return duration;
}

// Obstacle Detection function to stop motors
void distanceStop() {
  distance = distanceUltra();
  Serial.println(distance); // Check if it works
  if (distance <= 12) {
    digitalWrite(A_Direction, HIGH);
    analogWrite(A_Speed, 0);
    digitalWrite(B_Direction, HIGH);
    analogWrite(B_Speed, 0);
    delay(3000); // Stop time 
  }
}

// Line following
void lineFollowing() {
  int Center_Tra_Value = digitalRead(sensorPin);
  
  if (isMoving){
  if (Center_Tra_Value == Black) {
    // Go slightly left when black
    digitalWrite(A_Direction, HIGH);
    analogWrite(A_Speed, 40);
    digitalWrite(B_Direction, LOW);
    analogWrite(B_Speed, 50);
  } else {

    // Go slightly right when white
    digitalWrite(A_Direction, HIGH);
    analogWrite(A_Speed, 50);
    digitalWrite(B_Direction, LOW);
    analogWrite(B_Speed, 40);
  }
  }
  else{digitalWrite(A_Direction, HIGH);
      analogWrite(A_Speed, 0);
      digitalWrite(B_Direction, LOW);
      analogWrite(B_Speed, 0);   }
}

void setup() {
  Serial.begin(9600);

  pinMode(cablePin, INPUT_PULLUP);
  pinMode(sensorPin, INPUT);
  pinMode(sensorPin_2, INPUT);
  pinMode(A_Direction, OUTPUT);
  pinMode(A_Speed, OUTPUT);
  pinMode(B_Direction, OUTPUT);
  pinMode(B_Speed, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  Serial.println("Robot Ready");
}

void loop() {
  int pinState = digitalRead(cablePin);

  // Wait for tirette to start
  if (pinState == HIGH) { // HIGH = no connection between D7 and GND
    // Start
    if (!timerStarted) {
      startTime = millis();
      timerStarted = true;
    }

    if (digitalRead(sensorPin_2) == Black){
      digitalWrite(A_Direction, HIGH);
      analogWrite(A_Speed, 0);
      digitalWrite(B_Direction, LOW);
      analogWrite(B_Speed, 0); 
      isMoving = false;    
    //while (true) {/*stop the robot*/}
    }

    // Follow line
    if (timerStarted && millis() - startTime >= 2000UL) { // 2 seconds delay
      lineFollowing();
      done = true ; 
    }

    // Stop 
    if (done) {
      digitalWrite(A_Direction, HIGH);
      analogWrite(A_Speed, 0);
      digitalWrite(B_Direction, LOW);
      analogWrite(B_Speed, 0);      
    }

    // Obstacle detection
    if ( millis() - lastDistanceCheck >= 500UL) {
      lastDistanceCheck = millis();
      distanceStop();
    }
  } else {
    timerStarted = false;
    done = false;
  }
}