// For Discord channel
// Sketch by Matricata
// Not working yet

#include <Servo.h>
// #include <SoftwareSerial.h>

const int motorA1 = 2;
const int motorA2 = 3;
const int motorB1 = 4;
const int motorB2 = 5;

const int GREEN_BUTTON_PIN = 34;
const int YELLOW_BUTTON_PIN = 36;
const int RED_BUTTON_PIN = 32;

//начална позиция на бутони
int lastStateGREEN = LOW;
int lastStateYELLOW = LOW;
int lastStateRED = LOW;
int currentStateGREEN;
int currentStateYELLOW;
int currentStateRED;

int i;

#define S1Trig 11
#define S1Echo 10
#define S2Trig 9
#define S2Echo 8
#define S3Trig 7
#define S3Echo 6

#define left A0
#define right A1

long duration;
int distance;

#define MAX_SPEED 150
#define MIN_SPEED 75

#define MAX_DISTANCE 150
#define MIN_DISTANCE_BACK 5

void setup()
{
  Serial.begin(9600);
  // put your setup code here, to run once:
  //инициализиране на бутони
  pinMode(GREEN_BUTTON_PIN, INPUT_PULLUP);
  pinMode(YELLOW_BUTTON_PIN, INPUT_PULLUP);
  pinMode(RED_BUTTON_PIN, INPUT_PULLUP);
  //set the ir sensors as inputs
  pinMode(left, INPUT);
  pinMode(right, INPUT);
  //Set the Trig pins as output pins
  pinMode(S1Trig, OUTPUT);
  pinMode(S2Trig, OUTPUT);
  pinMode(S3Trig, OUTPUT);
  //Set the Echo pins as input pins
  pinMode(S1Echo, INPUT);
  pinMode(S2Echo, INPUT);
  pinMode(S3Echo, INPUT);
  //Set the motor pins as output
  pinMode(motorA1, OUTPUT);
  pinMode(motorA2, OUTPUT);
  pinMode(motorB1, OUTPUT);
  pinMode(motorB2, OUTPUT);
}

void loop() {
  // put your main code here, to run repeatedly:

  currentStateGREEN = digitalRead(GREEN_BUTTON_PIN);
  currentStateYELLOW = digitalRead(YELLOW_BUTTON_PIN);
  currentStateRED = digitalRead(RED_BUTTON_PIN);

  //Проверка кой бутон е натиснат
  //Button();
  //Sonic();

  if (lastStateGREEN == LOW && currentStateGREEN == HIGH) {
    Serial.println("The GREEN button is pressed");
    Sonic();
    delay(1000);

  }
  if (lastStateYELLOW == LOW && currentStateYELLOW == HIGH) {
    Serial.println("The YELLOW button is pressed");
    IRSensor();
    delay(1000);

  }
  if (lastStateRED == LOW && currentStateRED == HIGH) {
    Serial.println("The RED button is pressed");
    stop();
    delay(1000);

  }
  lastStateGREEN = currentStateGREEN;
  lastStateYELLOW = currentStateYELLOW;
  lastStateRED = currentStateRED;

}

//void Button(){
//}

int SensorOne() {
  // Ensure the trigger pin is set to LOW
  digitalWrite(S1Trig, LOW);
  delayMicroseconds(2);

  // Set the trigger pin to HIGH for 10 microseconds
  digitalWrite(S1Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(S1Trig, LOW);

  // Read the duration of the echo pin
  duration = pulseIn(S1Echo, HIGH);

  // Calculate the distance by dividing the duration by 2 (speed of sound) and then multiplying by 0.0344 (to convert to centimeters)
  distance = duration / 2 * 0.0344;
  /*
    Serial.print("Distance :");
    Serial.print(distance);
    Serial.print("cm");
    Serial.println();
  */
  return distance;
}

int SensorTwo() {
  // Ensure the trigger pin is set to LOW
  digitalWrite(S2Trig, LOW);
  delayMicroseconds(2);

  // Set the trigger pin to HIGH for 10 microseconds
  digitalWrite(S2Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(S2Trig, LOW);

  // Read the duration of the echo pin
  duration = pulseIn(S2Echo, HIGH);

  // Calculate the distance by dividing the duration by 2 (speed of sound) and then multiplying by 0.0344 (to convert to centimeters)
  distance = duration / 2 * 0.0344;
  /*
    Serial.print("Distance 2:");
    Serial.print(distance);
    Serial.print("cm");
    Serial.println();
  */
  return distance;
}

int SensorThree() {
  // Ensure the trigger pin is set to LOW
  digitalWrite(S3Trig, LOW);
  delayMicroseconds(2);

  // Set the trigger pin to HIGH for 10 microseconds
  digitalWrite(S3Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(S3Trig, LOW);

  // Read the duration of the echo pin
  duration = pulseIn(S3Echo, HIGH);

  // Calculate the distance by dividing the duration by 2 (speed of sound) and then multiplying by 0.0344 (to convert to centimeters)
  distance = duration / 2 * 0.0344;
  /*
    Serial.print("Distance 3:");
    Serial.print(distance);
    Serial.print("cm");
    Serial.println();
  */
  return distance;
}

void Sonic() {

  int leftDistance = SensorOne();
  int frontDistance = SensorTwo();
  int rightDistance = SensorThree();


  if (distance < MIN_DISTANCE_BACK) {
    moveBackward();
    Serial.println("backward");
  } else if (frontDistance < leftDistance && frontDistance < rightDistance && frontDistance < MAX_DISTANCE) {
    moveForward();
    Serial.println("forward");
  } else if (leftDistance < rightDistance && leftDistance < MAX_DISTANCE) {
    turnRight();
    Serial.println("right");
  } else if (rightDistance < MAX_DISTANCE) {
    turnLeft();
    Serial.println("left");
  } else {
    stop();
    Serial.println("stop");
  }
  //delay(500); // Delay for stability and to avoid excessive readings


  Serial.print("Front: ");
  Serial.print(frontDistance);
  Serial.print(" cm, Left: ");
  Serial.print(leftDistance);
  Serial.print(" cm, Right: ");
  Serial.print(rightDistance);
  Serial.println(" cm");

  delay(500);

}

void IRSensor() {

  Serial.println(digitalRead(left));
  Serial.println(digitalRead(right));

  if (digitalRead(left) == 0 && digitalRead(right) == 0) {
    //Forward
    moveForward();
    Serial.println("moving forward");

  }
  //line detected by left sensor
  else if (digitalRead(left) == 0 && !analogRead(right) == 0) {
    //turn left
    turnLeft();
    Serial.println("turning left");
  }
  //line detected by right sensor
  else if (!digitalRead(left) == 0 && digitalRead(right) == 0) {
    //turn right
    turnRight();
    Serial.println("turning right");
  }
  //line detected by none
  else if (!digitalRead(left) == 0 && !digitalRead(right) == 0) {
    //stop
    stop();
    Serial.println("stop");
  }

}

void moveBackward() {
  analogWrite(motorA1, MAX_SPEED);
  analogWrite(motorA2, LOW);
  analogWrite(motorB1, MAX_SPEED);
  analogWrite(motorB2, LOW);
}

void moveForward() {
  analogWrite(motorA1, LOW);
  analogWrite(motorA2, MAX_SPEED);
  analogWrite(motorB1, LOW);
  analogWrite(motorB2, MAX_SPEED);
}

void turnRight() {
  analogWrite(motorA1, LOW);
  analogWrite(motorA2, MAX_SPEED);
  analogWrite(motorB1, MAX_SPEED);
  analogWrite(motorB2, LOW);
}

void turnLeft() {
  analogWrite(motorA1, MAX_SPEED);
  analogWrite(motorA2, LOW);
  analogWrite(motorB1, LOW);
  analogWrite(motorB2, MAX_SPEED);
}

void stop() {
  analogWrite(motorA1, LOW);
  analogWrite(motorA2, LOW);
  analogWrite(motorB1, LOW);
  analogWrite(motorB2, LOW);
}