// Пример кода для организации простой коммуникации между роботами с использованием Bluetooth
// Данный код является учебным примером и требует адаптации под конкретное оборудование

#include <SoftwareSerial.h>

// Пины для Bluetooth модуля
#define BT_RX 10
#define BT_TX 11

// Создаем объект для работы с Bluetooth
SoftwareSerial bluetoothSerial(BT_RX, BT_TX);

// Константы для типов сообщений
#define MSG_COMMAND 'C'  // Команда
#define MSG_DATA 'D'     // Данные
#define MSG_ACK 'A'      // Подтверждение
#define MSG_ERROR 'E'    // Ошибка

// Буфер для приема сообщений
char messageBuffer[64];
int bufferIndex = 0;

// Структура для хранения команд и данных
struct RobotMessage {
  char type;
  int commandId;
  int parameter1;
  int parameter2;
  bool needAck;
};

// Функция инициализации коммуникации
void initCommunication() {
  bluetoothSerial.begin(9600);
  Serial.println("Communication initialized");
}

// Функция отправки сообщения
void sendMessage(char type, int commandId, int param1, int param2, bool needAck) {
  // Форматируем сообщение: тип,commandId,param1,param2,needAck
  String message = String(type) + "," + String(commandId) + "," + 
                   String(param1) + "," + String(param2) + "," + 
                   (needAck ? "1" : "0") + "\n";
                   
  bluetoothSerial.print(message);
  
  Serial.print("Sent: ");
  Serial.print(message);
}

// Функция отправки команды
void sendCommand(int commandId, int param1, int param2, bool needAck) {
  sendMessage(MSG_COMMAND, commandId, param1, param2, needAck);
}

// Функция отправки данных
void sendData(int dataId, int value1, int value2, bool needAck) {
  sendMessage(MSG_DATA, dataId, value1, value2, needAck);
}

// Функция отправки подтверждения
void sendAcknowledgement(int commandId) {
  sendMessage(MSG_ACK, commandId, 0, 0, false);
}

// Функция отправки сообщения об ошибке
void sendError(int errorCode, int commandId) {
  sendMessage(MSG_ERROR, commandId, errorCode, 0, false);
}

// Функция обработки входящего сообщения
void processMessage(const String& message) {
  // Парсинг сообщения
  int commaPos1 = message.indexOf(',');
  int commaPos2 = message.indexOf(',', commaPos1 + 1);
  int commaPos3 = message.indexOf(',', commaPos2 + 1);
  int commaPos4 = message.indexOf(',', commaPos3 + 1);
  
  if (commaPos1 < 0 || commaPos2 < 0 || commaPos3 < 0 || commaPos4 < 0) {
    Serial.println("Error parsing message: invalid format");
    return;
  }
  
  // Извлекаем части сообщения
  char type = message.charAt(0);
  int commandId = message.substring(commaPos1 + 1, commaPos2).toInt();
  int param1 = message.substring(commaPos2 + 1, commaPos3).toInt();
  int param2 = message.substring(commaPos3 + 1, commaPos4).toInt();
  bool needAck = (message.substring(commaPos4 + 1).toInt() == 1);
  
  // Создаем структуру сообщения
  RobotMessage msg = {type, commandId, param1, param2, needAck};
  
  // Обрабатываем сообщение в зависимости от типа
  switch (type) {
    case MSG_COMMAND:
      processCommand(msg);
      break;
    case MSG_DATA:
      processData(msg);
      break;
    case MSG_ACK:
      processAcknowledgement(msg);
      break;
    case MSG_ERROR:
      processError(msg);
      break;
    default:
      Serial.println("Unknown message type");
  }
  
  // Отправляем подтверждение, если оно требуется
  if (needAck) {
    sendAcknowledgement(commandId);
  }
}

// Функция обработки входящей команды
void processCommand(const RobotMessage& msg) {
  Serial.print("Command received: ID=");
  Serial.print(msg.commandId);
  Serial.print(", Param1=");
  Serial.print(msg.parameter1);
  Serial.print(", Param2=");
  Serial.println(msg.parameter2);
  
  // Здесь код для выполнения команды в зависимости от commandId
  // Например:
  switch (msg.commandId) {
    case 1: // Движение
      moveRobot(msg.parameter1, msg.parameter2);
      break;
    case 2: // Взаимодействие с объектом
      interactWithObject(msg.parameter1);
      break;
    // Другие команды...
    default:
      Serial.println("Unknown command ID");
  }
}

// Обработка данных
void processData(const RobotMessage& msg) {
  Serial.print("Data received: ID=");
  Serial.print(msg.commandId);
  Serial.print(", Value1=");
  Serial.print(msg.parameter1);
  Serial.print(", Value2=");
  Serial.println(msg.parameter2);
  
  // Здесь код для обработки полученных данных
}

// Обработка подтверждения
void processAcknowledgement(const RobotMessage& msg) {
  Serial.print("Acknowledgement for command ID=");
  Serial.println(msg.commandId);
  
  // Здесь код для обработки подтверждения
}

// Обработка ошибки
void processError(const RobotMessage& msg) {
  Serial.print("Error for command ID=");
  Serial.print(msg.commandId);
  Serial.print(", Error code=");
  Serial.println(msg.parameter1);
  
  // Здесь код для обработки ошибки
}

// Проверка входящих сообщений
void checkIncomingMessages() {
  while (bluetoothSerial.available()) {
    char c = bluetoothSerial.read();
    
    // Если конец строки, обрабатываем сообщение
    if (c == '\n') {
      messageBuffer[bufferIndex] = '\0'; // Добавляем конец строки
      processMessage(String(messageBuffer));
      bufferIndex = 0; // Сбрасываем индекс буфера
    } else if (bufferIndex < 63) { // Проверка переполнения буфера
      messageBuffer[bufferIndex++] = c;
    }
  }
}

// Примеры функций управления роботом (должны быть реализованы в зависимости от конкретной платформы)
void moveRobot(int direction, int speed) {
  // Код для управления движением робота
  Serial.print("Moving robot: Direction=");
  Serial.print(direction);
  Serial.print(", Speed=");
  Serial.println(speed);
}

void interactWithObject(int actionType) {
  // Код для взаимодействия с объектами
  Serial.print("Interacting with object: Action=");
  Serial.println(actionType);
}

void setup() {
  Serial.begin(9600);
  initCommunication();
  
  // Другие инициализации...
}

void loop() {
  // Проверяем входящие сообщения
  checkIncomingMessages();
  
  // Другие задачи робота...
  
  delay(10); // Небольшая задержка для стабильности
}

# Пример кода на Python для координации движения нескольких роботов
# Данный код является учебным примером и должен быть адаптирован к конкретной платформе

import math
import time

# Класс для представления положения робота
class Position:
    def __init__(self, x=0, y=0, angle=0):
        self.x = x
        self.y = y
        self.angle = angle
    
    def distance_to(self, other_position):
        """Рассчитывает расстояние до другой позиции"""
        return math.sqrt((self.x - other_position.x)**2 + (self.y - other_position.y)**2)
    
    def angle_to(self, other_position):
        """Рассчитывает угол до другой позиции"""
        return math.degrees(math.atan2(other_position.y - self.y, other_position.x - self.x))

# Класс для представления робота
class Robot:
    def __init__(self, robot_id, initial_position=Position()):
        self.id = robot_id
        self.position = initial_position
        self.target_position = None
        self.speed = 0
        self.state = "idle"  # idle, moving, working
        self.task = None
    
    def set_target(self, target_position):
        """Устанавливает целевую позицию для робота"""
        self.target_position = target_position
        self.state = "moving"
        
    def update_position(self, new_position):
        """Обновляет текущую позицию робота"""
        self.position = new_position
        
        # Проверяем, достиг ли робот целевой позиции
        if (self.target_position and 
            self.position.distance_to(self.target_position) < 0.1):
            self.state = "idle"
            self.target_position = None
            return True
        return False
    
    def assign_task(self, task):
        """Назначает задачу роботу"""
        self.task = task
        self.state = "working"
    
    def complete_task(self):
        """Отмечает задачу как выполненную"""
        self.task = None
        self.state = "idle"

# Класс для координации группы роботов
class RobotCoordinator:
    def __init__(self):
        self.robots = {}
        self.tasks = []
    
    def add_robot(self, robot):
        """Добавляет робота в систему координации"""
        self.robots[robot.id] = robot
    
    def add_task(self, task):
        """Добавляет задачу в очередь"""
        self.tasks.append(task)
    
    def get_idle_robots(self):
        """Возвращает список свободных роботов"""
        return [robot for robot in self.robots.values() if robot.state == "idle"]
    
    def assign_tasks(self):
        """Распределяет задачи между свободными роботами"""
        idle_robots = self.get_idle_robots()
        
        while idle_robots and self.tasks:
            task = self.tasks.pop(0)  # Берем первую задачу из очереди
            
            # Находим ближайшего робота к месту выполнения задачи
            nearest_robot = None
            min_distance = float('inf')
            
            for robot in idle_robots:
                distance = robot.position.distance_to(task.position)
                if distance < min_distance:
                    min_distance = distance
                    nearest_robot = robot
            
            if nearest_robot:
                nearest_robot.set_target(task.position)
                nearest_robot.assign_task(task)
                idle_robots.remove(nearest_robot)
                print(f"Робот {nearest_robot.id} назначен на задачу {task.id} в позиции ({task.position.x}, {task.position.y})")
    
    def coordinate_movement(self):
        """Координирует движение роботов для избежания столкновений"""
        moving_robots = [robot for robot in self.robots.values() if robot.state == "moving"]
        
        # Проверяем каждую пару движущихся роботов
        for i in range(len(moving_robots)):
            for j in range(i + 1, len(moving_robots)):
                robot1 = moving_robots[i]
                robot2 = moving_robots[j]
                
                # Если роботы находятся слишком близко друг к другу
                if robot1.position.distance_to(robot2.position) < 2.0:
                    # Вычисляем векторы направления движения
                    vector1 = (robot1.target_position.x - robot1.position.x, 
                              robot1.target_position.y - robot1.position.y)
                    vector2 = (robot2.target_position.x - robot2.position.x, 
                              robot2.target_position.y - robot2.position.y)
                    
                    # Если роботы движутся навстречу друг другу
                    dot_product = vector1[0]*vector2[0] + vector1[1]*vector2[1]
                    if dot_product < 0:
                        # Определяем, какой робот должен уступить дорогу
                        # Например, по приоритету задачи или ID робота
                        if robot1.id > robot2.id:
                            self.adjust_path(robot1, robot2)
                        else:
                            self.adjust_path(robot2, robot1)
    
    def adjust_path(self, robot, obstacle_robot):
        """Корректирует путь робота для избежания столкновения"""
        # Временно изменяем целевую точку, чтобы обойти препятствие
        # Это упрощенный пример, в реальности нужен более сложный алгоритм планирования пути
        
        # Вычисляем вектор от препятствия к роботу
        dx = robot.position.x - obstacle_robot.position.x
        dy = robot.position.y - obstacle_robot.position.y
        
        # Нормализуем вектор и создаем смещение
        length = math.sqrt(dx*dx + dy*dy)
        if length > 0:
            dx = dx / length * 3.0  # Смещение на 3 единицы
            dy = dy / length * 3.0
        
        # Создаем промежуточную точку в стороне от препятствия
        intermediate_position = Position(
            obstacle_robot.position.x + dx,
            obstacle_robot.position.y + dy
        )
        
        # Временно устанавливаем эту точку как целевую
        original_target = robot.target_position
        robot.set_target(intermediate_position)
        
        # Запоминаем оригинальную точку, чтобы вернуться к ней позже
        # В реальной системе нужно сохранять весь маршрут
        print(f"Робот {robot.id} корректирует путь для избежания столкновения с роботом {obstacle_robot.id}")

# Класс для представления задачи
class Task:
    def __init__(self, task_id, position, priority=0):
        self.id = task_id
        self.position = position
        self.priority = priority
        self.status = "pending"  # pending, assigned, completed

# Пример использования
if __name__ == "__main__":
    # Создаем координатора
    coordinator = RobotCoordinator()
    
    # Создаем роботов
    robot1 = Robot(1, Position(0, 0))
    robot2 = Robot(2, Position(10, 0))
    
    # Добавляем роботов в систему координации
    coordinator.add_robot(robot1)
    coordinator.add_robot(robot2)
    
    # Создаем задачи
    task1 = Task(1, Position(5, 5))
    task2 = Task(2, Position(8, 8))
    
    # Добавляем задачи в очередь
    coordinator.add_task(task1)
    coordinator.add_task(task2)
    
    # Распределяем задачи
    coordinator.assign_tasks()
    
    # Имитация движения и координации (в реальности это было бы в цикле)
    for i in range(5):
        print(f"\nИтерация {i+1}:")
        
        # Обновляем позиции роботов (в реальности это было бы фактическое движение)
        if robot1.state == "moving":
            robot1.update_position(Position(
                robot1.position.x + (robot1.target_position.x - robot1.position.x) * 0.2,
                robot1.position.y + (robot1.target_position.y - robot1.position.y) * 0.2
            ))
            print(f"Робот 1: позиция ({robot1.position.x:.1f}, {robot1.position.y:.1f})")
        
        if robot2.state == "moving":
            robot2.update_position(Position(
                robot2.position.x + (robot2.target_position.x - robot2.position.x) * 0.2,
                robot2.position.y + (robot2.target_position.y - robot2.position.y) * 0.2
            ))
            print(f"Робот 2: позиция ({robot2.position.x:.1f}, {robot2.position.y:.1f})")
        
        # Координируем движение для избежания столкновений
        coordinator.coordinate_movement()
        
        # Имитация завершения задач
        for robot in [robot1, robot2]:
            if (robot.task and robot.state == "working" and 
                robot.position.distance_to(robot.task.position) < 0.5):
                print(f"Робот {robot.id} выполнил задачу {robot.task.id}")
                robot.complete_task()
        
        # Назначаем новые задачи, если они есть
        coordinator.assign_tasks()
        
        # Задержка для наглядности
        time.sleep(1)