#define LEFT_PIN 19
#define RIGHT_PIN 18

#define TRIG1 12
#define ECHO1 13

#define TRIG3 25
#define ECHO3 26

#define TRIG4 27
#define ECHO4 33

#define FREE_SPACE 10  // см, больше - свободно

void printMotorsJson() {
  Serial.printf("{\"motor_left\":%d,\"motor_right\":%d}\n",
                digitalRead(LEFT_PIN),
                digitalRead(RIGHT_PIN));
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(TRIG1, OUTPUT);
  pinMode(ECHO1, INPUT);

    pinMode(TRIG3, OUTPUT);
  pinMode(ECHO3, INPUT);

    pinMode(TRIG4, OUTPUT);
  pinMode(ECHO4, INPUT);

  pinMode(LEFT_PIN, OUTPUT);
  pinMode(RIGHT_PIN, OUTPUT);

  Serial.println("Project ready");
}

long getDistance(int trig, int echo) {
  digitalWrite(trig, LOW); delayMicroseconds(2);
  digitalWrite(trig, HIGH); delayMicroseconds(10);
  digitalWrite(trig, LOW);
  return pulseIn(echo, HIGH) * 0.034 / 2;  // см
}

void setMotors(int left_motor, int right_motor){
  digitalWrite(LEFT_PIN, left_motor);
  digitalWrite(RIGHT_PIN, right_motor);
}

void motor_cmds(long front, long left, long right){
  bool freeFront = (front > FREE_SPACE);
  bool freeLeft = (left > FREE_SPACE);
  bool freeRight = (right > FREE_SPACE);

  if (freeFront) {
  setMotors(1, 1);  // вперед
} else if (freeLeft && freeRight) {
  setMotors(1, 0);  // направо
} else if (freeRight) {
  setMotors(1, 0);  // направо
} else if (freeLeft) {
  setMotors(0, 1);  // налево
} else {
  setMotors(1, 0);  // стоп!
}


}

void loop() {
  long forward_value = getDistance(TRIG1, ECHO1);
  long left_value = getDistance(TRIG3, ECHO3);
  long right_value = getDistance(TRIG4, ECHO4);
  
  motor_cmds(forward_value, left_value, right_value);
  printMotorsJson();
  delay(100);
}