#include "Wire.h"  // библиотека для управления I2C
#include "HMC5883L.h" // библеотека для компаса
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_PCD8544.h> // библиотека для управления LCD экрана
#include <SoftwareSerial.h> // библиотека для работы с Serial через дискретные порты
#include <AFMotor.h> // библиотека для управления DC двигателями
#include <Servo.h> // библиотека для управления сервоприводами

#define LED_STOP_PIN   23 // контакт STOP-светодиода
#define BUTTON_PIN     22 // контакт кнопки
#define TRIGGER_PIN    24 // контакт уз излучателя
#define ECHO_PIN       25 // контакт уз приемника

Adafruit_PCD8544 display = Adafruit_PCD8544(26, 27, 28, 30, 29); // контакты LCD экрана (SCLK, DIN, D/C, CS, RST)
HMC5883L compass; // режим работаы HMC5883L

AF_DCMotor motor1(1); //канал М1 на Motor Shield — передний правый 
AF_DCMotor motor2(2); //канал М2 на Motor Shield — задний правый 
AF_DCMotor motor3(3); //канал М3 на Motor Shield — передний левый
AF_DCMotor motor4(4); //канал М4 на Motor Shield — задний левый 
Servo servo; // сервопривод

SoftwareSerial BTSerial(A14, A15); // RX, TX для BT модуля

char bt_cmd; // переменную для команд Bluetooth 
int mode = 0; // переменная режима работы
const int servo_array[7]={2,25,50,80,110,135,158}; // Массив для хранения углов поворота сервопривода (шаг 30 градусов, 5 шагов) 
int distance_array[7]; // Массив для хранения данных о расстоянии под различными углами поворота сервопривода 
int servo_int; // Переменная для циклов перебора значения массивов vservo_array и vHC_SR04_array 
int maxarrayindex; // Переменные для цикла поиска максимального значения в массивах 
int maxarrayvalue;
int compass_value; // показание компаса 1
int compass_value_2; // целевое показание компаса
int distance_min = 18;
int distance;
int distance_2;


void setup() { 
  BTSerial.begin(9600); // устанавливаем скорость передачи данных по Bluetooth 
  Wire.begin();
  servo.attach(9); // пин сервопривода (на плате расширения - SERVO2)
  servo.write(80); // поворот сервопривода по ходу движения (с учетом крена)servo_h.write(88); // поворот сервопривода по ходу движения (с учетом крена)
  pinMode(LED_STOP_PIN, OUTPUT); // режим работы контакта для светодиода
  pinMode(BUTTON_PIN, INPUT); // режим работы контакта для кнопки
  pinMode(TRIGGER_PIN, OUTPUT); // режим работы контакта для уз излучателя
  pinMode(ECHO_PIN, INPUT); // режим работы контакта для уз приемника
  compass = HMC5883L();
  compass.SetScale(1.3); // установка чувствительности компаса
  compass.SetMeasurementMode(Measurement_Continuous); // устанавливаем непрерывный режим работы компаса
  display.begin();
  display.setContrast(45); // контраст LCD экрана
  LCD_mode();
}

void loop(){
  if (digitalRead(BUTTON_PIN) == HIGH){
    mode = mode + 1;
    if (mode > 3){mode = 0;}
    dc_release();
    LCD_mode();
    delay(1500);
  }
  switch (mode){
    case 0:
      servo.detach();
      set_speed(255,255);
      mode_bt(); // дистанционное управление
      break;
    case 1:
      servo.attach(9);
      set_speed(255,255);
      mode_auto(); // автоматическая езда
      break;
    case 2:
      servo.detach();
      set_speed(255,255);
      compass_test(); // тест компаса
      break;
    case 3:
      break; // режим ожидания
  } 
}


void mode_auto(){ // автоматическая езда
  if (u_distance() < distance_min) { 
    dc_release(); // останавливаем двигатели 
    digitalWrite(LED_STOP_PIN, HIGH); // зажигаем светодиод
    for (servo_int = 0; servo_int < 7; servo_int = servo_int + 1) { // крутим датчик, измеряя расстояния и занося данные в массив 
      servo.write(servo_array[servo_int]); 
      delay(50); 
      distance_array[servo_int] = u_distance();
    } 
    servo.write(80); 
    delay(150); 
// поиск позиции с максимальным значением в массиве
    maxarrayindex = 0; 
    maxarrayvalue = 0;    
    for (servo_int = 0; servo_int < 7; servo_int = servo_int + 1) {     
      if (distance_array[servo_int] > maxarrayvalue) { 
        maxarrayindex = servo_int; 
        maxarrayvalue = distance_array[servo_int]; 
      } 
    } 
// если максимальное значение массива меньше минимального расстояния, то зажигаем светодиод и едем назад 
    if (distance_array[maxarrayindex] < distance_min) { 
      digitalWrite(LED_STOP_PIN, HIGH); 
      dc_backward(); 
      delay(200); 
    } 
// если индекс максимального значения массива меньше 3, то поворачиваем вправо, иначе влево 
    if (maxarrayindex < 3) {dc_right(); delay(250);} 
      else {dc_left(); delay(250);} 
  } 
  else { // гасим светодиод и едем прямо
    digitalWrite(LED_STOP_PIN, LOW); 
    dc_forward();
    } 
}


void mode_bt(){ // управление по bluetooth 
  u_distance();
  if (BTSerial.available()) // если есть данные с BT
  { 
    if (distance < distance_min) {dc_release(); digitalWrite(LED_STOP_PIN, HIGH);} // зажигаем светодиод
      else {digitalWrite(LED_STOP_PIN, LOW);} // гасим светодиод
    bt_cmd = (char)BTSerial.read(); // Читаем команды и заносим их в переменную. char преобразует код символа команды в символ  
    
// движение 
    if (bt_cmd == 'F' && distance >= distance_min) {dc_forward();} 
    if (bt_cmd == 'B') {dc_backward();} 
    if (bt_cmd == 'L') {dc_left();}     
    if (bt_cmd == 'R') {dc_right();}  
    if (bt_cmd == 'S') {dc_release();} 
// управление скоростью 
    if (bt_cmd == '0') {set_speed(115,115);} 
    if (bt_cmd == '1') {set_speed(130,130);} 
    if (bt_cmd == '2') {set_speed(145,145);} 
    if (bt_cmd == '3') {set_speed(160,160);} 
    if (bt_cmd == '4') {set_speed(175,175);} 
    if (bt_cmd == '5') {set_speed(180,180);} 
    if (bt_cmd == '6') {set_speed(195,195);} 
    if (bt_cmd == '7') {set_speed(210,210);} 
    if (bt_cmd == '8') {set_speed(225,225);} 
    if (bt_cmd == '9') {set_speed(240,240);} 
    if (bt_cmd == 'q') {set_speed(255,255);}
// прочие телодвижения
    if (bt_cmd == 'X' || bt_cmd == 'x') {mode = 1; return;} // переход в авторежим
  }
}


void compass_test(){ // тест компаса   
  compass_value = compass_measure();
  compass_value_2 = (compass_value + 90) % 360;
  LCD_mode();
  delay(2500);
  do{
    dc_right();
    delay(40);
    compass_value = compass_measure();
    if (compass_value_2 < 90 && compass_value > 270) {compass_value = compass_value - 360;}
    }
  while (compass_value <= compass_value_2);
  dc_release();
}
  
    
void dc_forward() { // функция движения вперед
  motor1.run(FORWARD); 
  motor2.run(FORWARD); 
  motor3.run(FORWARD); 
  motor4.run(FORWARD);
}

void dc_backward() { // функция движения назад
  motor1.run(BACKWARD); 
  motor2.run(BACKWARD); 
  motor3.run(BACKWARD); 
  motor4.run(BACKWARD);
}

void dc_right() { // функция поворота влево
  motor1.run(FORWARD); 
  motor2.run(FORWARD); 
  motor3.run(BACKWARD); 
  motor4.run(BACKWARD);
}

void dc_left() { // функция поворота вправо
  motor1.run(BACKWARD); 
  motor2.run(BACKWARD); 
  motor3.run(FORWARD); 
  motor4.run(FORWARD); 
}

void dc_release(){ // функция остановки
  motor1.run(RELEASE); 
  motor2.run(RELEASE); 
  motor3.run(RELEASE); 
  motor4.run(RELEASE);
}

void set_speed(int spdL,int spdR){ // функция изменения скорости
  motor1.setSpeed(spdL); 
  motor2.setSpeed(spdR); 
  motor3.setSpeed(spdL); 
  motor4.setSpeed(spdR); 
}

int u_distance(){ // функция измерение расстояния
  digitalWrite(TRIGGER_PIN, HIGH); // Подаем сигнал на выход датчика 
  delayMicroseconds(10); // Удерживаем 10 микросекунд 
  digitalWrite(TRIGGER_PIN, LOW); // убираем сигнал 
  distance = pulseIn(ECHO_PIN, HIGH, 20000)/58; // Замеряем длину импульса и пересчитываем в сантиметры 
  digitalWrite(TRIGGER_PIN, HIGH); // Подаем сигнал на выход датчика 
  delayMicroseconds(10); // Удерживаем 10 микросекунд 
  digitalWrite(TRIGGER_PIN, LOW); // убираем сигнал 
  distance_2 = pulseIn(ECHO_PIN, HIGH, 20000)/58; // Замеряем длину импульса и пересчитываем в сантиметры 
  if (distance < distance_2) {distance = distance_2;}
  return distance; // Возвращаем значение 
}


float compass_measure(){ // функция расчета угла поворота
  MagnetometerScaled scaled = compass.ReadScaledAxis();
  int MilliGauss_OnThe_XAxis = scaled.XAxis;
  float heading = atan2(scaled.YAxis, scaled.XAxis);
// корректируем значения с учетом знаков
  if(heading < 0) heading += 2*PI;
  if(heading > 2*PI) heading -= 2*PI;
  heading = (heading * 180/M_PI); // переводим радианы в градусы
  return heading;
}


void LCD_mode(){ // функция вывода на LCD
  display.clearDisplay();
  display.setRotation(2); // переворот изображения на 180 градусов
  display.setTextSize(1);
  display.setTextColor(BLACK);
  display.setCursor(0,0);
  display.println("Drive mode:");
  display.println();
  display.setTextSize(2);
  display.setTextColor(BLACK);
  if (mode == 0) {
    display.println("   BT");
    display.println("CONTROL");
    display.display();
    }
  if (mode == 1) {
    display.print("  AUTO");
    display.display();
    }
  if (mode == 2) {
    display.print("COMPASS");
    display.setTextSize(1);
    if (compass_value != 0 && compass_value_2 !=0){
      display.print("from: ");
      display.println(compass_value);
      display.print("  to: ");
      display.println(compass_value_2);
      }
    display.display();
    }
  if (mode == 3) {
    display.print("  STOP");
    display.display();
    }   
}