// Program Radar dla Arduino

#include <SoftwareServo.h>
#include <NewSoftSerial.h>

// Cyfrowe PINy do których podłączyłeś czujnik
#define SENSOR_PIN_TX 2
#define SENSOR_PIN_RX 3

// Cyfrowy PIN do którego podłączyłęś sygnał sterujący PWM serwomechanizmu
#define SERVO_PIN 4

// Zmienna przechowująca aktualny kierune w jakim ma być skierowany czujnik
int direction = 90;
// Zmienna przechowująca informacje o tym czy zarządano podania odległości
byte read_distance = 0;

// Deklaracja obiektu komunikacji szeregowej z czujnikiem
NewSoftSerial sensor_serial(SENSOR_PIN_TX, SENSOR_PIN_RX, true);
// Deklaracja obiektu sterowania serwomechanizmem
SoftwareServo sensor_servo;

// Funkcja zwracająca odległość w calach
int get_distance()
{
  char data[4] = "000";
  char header = ' ';
  while (header != 'R')
    header = sensor_serial.read();
  while (sensor_serial.available() < 4);
  for (byte i = 0; i < 3; i++)
    data[i] = sensor_serial.read();
  header = sensor_serial.read();
  if (header != 13)
    return -1;
  return atoi(data);
}

// Funkcja wysyłająca liczbę w formacie "nnn" (3-cyfrowym)
void send_number(int number)
{
  if (number < 100)
    Serial.print("0");
  if (number < 10)
    Serial.print("0");
  Serial.print(number, DEC);
}

// Funkcja wysyłająca do komputera dane o odległości
void send_data(int distance)
{
  Serial.print("D");
  send_number(distance);
  Serial.println("");
}

// Funkcja odbierająca rozkazy z komputera
// Argument: timeout - czas w [ms] po którym przerywane są próby odczytu
int get_direction(unsigned long timeout=1000)
{
  char data[4] = "000";
  char header = ' ';
  // Zapamiętanie aktualnego czasu
  unsigned long time = millis();
  // Pobranie nagłówka
  header = Serial.read();
  // Sprawdzenie czy jest to żadanie odległości
  if (header == 'D')
  {
    read_distance = 1;
    return -4;
  }
  // Sprawdzenie czy jest to nagłówek rozkazu kierunku
  if (header != 'R')
    return -1; // Jeśli inny to zwrócenie błędu o kodzie -1
  // Oczekiwanie na pojawienie się 3 bajtów danych
  while (Serial.available() < 4)
  {
    // Sprawdzenie czy upłynął dopuszczalny czas oczekiwania
    if ((millis() - time) > timeout)
      return -2; // Jeśli tak to zwrócenie kodu błędu -2
  }
  // Odczyt cyfr odległości
  for (byte i = 0; i < 3; i++)
    data[i] = Serial.read();
  // Sprawdzenie poprawności zakończenia nadawania danych
  header = Serial.read();
  if (header != '\n')
    return -3; // Zwrócenie -3 oznacza niepoprawne zakończenie
  return atoi(data);
}

// Funkcja sprawdza czy wysłano rozkaz o obrocie i wykonanie go
void rotate()
{
  // Sprawdzenie czy w buforze czekają jakieś dane
  while (Serial.available())
  {
    direction = get_direction();
    if (direction >= 0)
      // Jeśli dane były poprawne to ustawienie kąta serwomechanizmu
      sensor_servo.write(direction);
  }
}

// Czyszczenie bufora komunikacji programowej z czujnikiem
void clear_buffer()
{
  while (sensor_serial.available())
    sensor_serial.flush();
}

// Funkcja zwraca odległość jeśli tego zażądano
void distance()
{
  // Sprawdzenie czy żądano zwrócenia odległości
  if (read_distance == 1)
  {
    // Aktywacja czujnika
    digitalWrite(SENSOR_PIN_RX, HIGH);
    // 20 us dla czujnika na określenie odległości
    delayMicroseconds(20);
    // Oczekiwanie na dane z czujnika
    while (sensor_serial.available())
    {
      int distance = get_distance();
      if (distance > 0)
      {
        // Jeśli czujnik zwrócił poprawne dane to wysłanie ich do komputera
        send_data(distance);
        // Skasowanie żądania o podanie odległości
        read_distance = 0;
        // Zatrzymanie pracy czujnika
        digitalWrite(SENSOR_PIN_RX, LOW);
        // Wyczyszczenie bufora
        clear_buffer();
      }
    }
  }
}


void setup()
{
  Serial.begin(9600);
  pinMode(SENSOR_PIN_TX, INPUT);
  pinMode(SENSOR_PIN_RX, OUTPUT);
  digitalWrite(SENSOR_PIN_RX, LOW);
  sensor_serial.begin(9600);
  // Kalibracja serwomechanizmu - minimalny i maksymalny czas impulsu w us
  sensor_servo.setMinimumPulse(560);
  sensor_servo.setMaximumPulse(2550);
  // Ustalenie pinu sterującego serwomechanizm
  sensor_servo.attach(SERVO_PIN);
  // Ustawienie serwomechanizmu na środek
  sensor_servo.write(90);
  // Odczekanie na ustawienie serwomechanizmu i uruchomienie czujnika
  delay(1000);
  // Czyszczenie bufora
  clear_buffer();
}


void loop()
{
  rotate();
  distance();
  
  // Wysłanie impulsu do serwomechanizmu
  SoftwareServo::refresh();
  delay(10);
}