/*
  Robotika – Lesson 4 · LEVEL 4: Telemetry - the car talks back
  -------------------------------------------------------------
  Bluetooth is TWO-WAY. Level 3 only listened. Now the car also REPORTS
  what it sees, 5 times a second, as one line of text:

      D=23,M=F,S=160
      |    |   '-- S = speed (0..255)
      |    '------ M = current move (F/B/L/R/S)
      '----------- D = distance in cm (999 = nothing in front)

  One line = one message. key=value pairs, separated by commas -
  simple for any program (even a web page) to read.

  New commands:  '1'..'9' = speed (1 slow ... 9 full)
  New idea:      millis() - a clock that does NOT stop the program.
                 delay(200) would freeze the car and miss commands.

  Wiring: exactly as Level 3.
  !! Unplug the HC-05 data wires (D0/D1) while uploading.
*/

const int trigPin = 9;
const int echoPin = 10;
const int ENA = 5, IN1 = 2, IN2 = 3, IN3 = 4, IN4 = 7, ENB = 6;

const int STOP_CM = 15;
const unsigned long REPORT_MS = 200;   // send a report every 200 ms

int speed = 160;                // not const any more - commands change it
char move = 'S';
int distanceCm = 999;
unsigned long lastReport = 0;

void setup() {
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(ENA, OUTPUT); pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  pinMode(ENB, OUTPUT); pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT);
  stopCar();
}

void loop() {
  // 1) INPUT: commands from the computer
  if (Serial.available() > 0) {
    char c = toupper(Serial.read());
    if (c == 'F' || c == 'B' || c == 'L' || c == 'R' || c == 'S') {
      move = c;
    } else if (c >= '1' && c <= '9') {
      speed = map(c - '0', 1, 9, 80, 255);   // '1' -> 80 ... '9' -> 255
    }
    // anything else ('\n', '\r', typos) is ignored
  }

  // 2) SENSOR + SAFETY
  distanceCm = getDistanceCm();
  if (move == 'F' && distanceCm <= STOP_CM) move = 'S';

  // 3) OUTPUT: motors
  switch (move) {
    case 'F': forward();   break;
    case 'B': backward();  break;
    case 'L': turnLeft();  break;
    case 'R': turnRight(); break;
    default:  stopCar();   break;
  }

  // 4) TELEMETRY: report without stopping the program
  if (millis() - lastReport >= REPORT_MS) {
    lastReport = millis();
    Serial.print("D=");  Serial.print(distanceCm);
    Serial.print(",M="); Serial.print(move);
    Serial.print(",S="); Serial.println(speed);
  }
}

// ---------- the four moves (now with a changing speed) ----------
void drive(int a1, int a2, int b1, int b2) {
  digitalWrite(IN1, a1); digitalWrite(IN2, a2);
  digitalWrite(IN3, b1); digitalWrite(IN4, b2);
  analogWrite(ENA, speed); analogWrite(ENB, speed);
}
void forward()   { drive(HIGH, LOW,  HIGH, LOW);  }
void backward()  { drive(LOW,  HIGH, LOW,  HIGH); }
void turnRight() { drive(HIGH, LOW,  LOW,  HIGH); }
void turnLeft()  { drive(LOW,  HIGH, HIGH, LOW);  }

void stopCar() {
  analogWrite(ENA, 0); analogWrite(ENB, 0);
  digitalWrite(IN1, LOW); digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW); digitalWrite(IN4, LOW);
}

int getDistanceCm() {
  digitalWrite(trigPin, LOW);  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH); delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH, 25000);
  if (duration == 0) return 999;
  return duration * 0.0343 / 2;
}