//including libraries and defining the address of devices
#include <Servo.h>
#include <Wire.h>
#define slave_move 0x09 //nmber may be changed based on needed value
#define slave_camera 0x10 //number may be changed based on needed value
#define Master 0x08 // number may be changed based on needed value

//setting up integers, numbers, and booleans
bool outside = true;
bool front = false;
String stringRead;
int intRead;
int left1 = 8;
int left2 = 7;
int speedL = 9;
int right1 = 4;
int right2 = 2;
int speedR = 6;
int trig_pin = 10;         //trigger pin
int echo_pin = 11;         //echo pin
long duration;             //time it took for sound to travel
float sound_speed = 0.0343;//constant speed of sound per second
float dist_val;            //object distance value


void setup() {
  Wire.begin(slave_move);      //set up connection as movement slave device
  Wire.onReceive(receiveData); //accepting data from camera
  Serial.begin(9600);
  pinMode(left1, OUTPUT);      //wheels
  pinMode(left2, OUTPUT);      //wheels
  pinMode(speedL, OUTPUT);     //wheels
  pinMode(right1, OUTPUT);     //wheels
  pinMode(right2, OUTPUT);     //wheels
  pinMode(speedR, OUTPUT);     //wheels
  pinMode(echo_pin, INPUT);    //ultrasonic sensor
  pinMode(trig_pin, OUTPUT);   //ultrasonic sensor
}


void receiveData(int byteCount) {  //void for receiving data from the camera
int stringLength;
Wire.requestFrom(slave_camera, 1);
if (Wire.available()) {
    stringLength = Wire.read();
}

for (int i = 0; i < stringLength; i++) {
    if (Wire.available()) {
        stringRead += (char)Wire.read();
    }
}

Wire.requestFrom(slave_camera, sizeof(intRead));
if (Wire.available()) {
    Wire.readBytes((char*)&intRead, sizeof(intRead));
}

}

  //esp32 cam code start
    //#NoSuchThingAsACodeForTheCamera
  //esp32 cam code end

  //esp32 cam movement code start
    //#KumaKumaPain
  //esp32 cam movement code end

  //wheels code start
  void forward() { //wheels moving forward at full speed
  digitalWrite(left1, HIGH);
  digitalWrite(left2, LOW);
  analogWrite(speedL, 255);
  digitalWrite(right1, HIGH);
  digitalWrite(right2, LOW);
  analogWrite(speedR, 255);
}

void stop() { //wheels stopping
  digitalWrite(left1, LOW);
  digitalWrite(left2, LOW);
  digitalWrite(right1, LOW);
  digitalWrite(right2, LOW);
}

void turnLeft(int speed) { //wheels turning left at given speed
  digitalWrite(left1, LOW);
  digitalWrite(left2, HIGH);
  digitalWrite(right1, HIGH);
  digitalWrite(right2, LOW);
  analogWrite(speedL, speed);
  analogWrite(speedR, speed);
}

void turnRight() { //wheels turning right at full speed
  digitalWrite(left1, HIGH);
  digitalWrite(left2, LOW);
  digitalWrite(right1, LOW);
  digitalWrite(right2, HIGH);
  analogWrite(speedL, 255);
  analogWrite(speedR, 255);
}
  //wheels code end

void loop() {

  //ultrasonic sensor code start (moved to the end)

  //ultrasonic sensor code end

//if(outside) {
  //if(dataRead >= 250) { //change values into correct central acceptable area
  //stop();
  //turnLeft();
  //}
  //else if(dataRead <= 230) { //change values into correct central acceptable area
  //stop();
  //turnRight();
  //}
  //else if(dataRead > 230; dataRead < 250) { //change values into correct central acceptable area
  //stop();
  //forward();
  //}
  //if(dist_val < 20) { //change value to correct distance value
  //stop();
  //outside = false;
  //}
//}
for(int i = 0; i < 10  ; i++) { //code for spinning machine to scan for objects 
  turnLeft(100);
  if(intRead > 0; intRead < 96) { //code if an object is detected and if no object is detected within 15 seconds, stop the machine
    continue;
    }
    else{
    delay(15000);
    stop();
    }
  }
  if(intRead > 0; intRead < 46) { //code to move the object to the center or to allign machine directly to object
    stop();
    turnLeft(255);
  }
  else if(intRead < 96; intRead > 50) {
    stop();
    turnRight();
  }
  else if(intRead >= 46; intRead <= 50) {
    stop();
  }
    
  


if(intRead >= 46; intRead <= 50) { //code that decides whether or not the machine goes forward
  if(dist_val >= 20) { //adjust value for correct distance
    front = true;
  }
  else{
    front = false;
  }
}

if(front) { //front movement code controlled by decision of code above
  forward();
}
else{stop();}

//code for ultrasonic sensor in constant detection
digitalWrite (trig_pin, LOW);
delayMicroseconds(2);
digitalWrite (trig_pin, HIGH);
delayMicroseconds(10);
digitalWrite (trig_pin, LOW);
duration=pulseIn(echo_pin, HIGH);
dist_val = duration*sound_speed/2;
//Serial.print("distance = ");
//Serial.println(dist_val);
delay(100);

}

void dataSend() { //code for sending distance value to master device
  int distance = dist_val;
  Wire.beginTransmission(Master);
  Wire.write(distance);
  Wire.endTransmission();
}


//end of the code