#include <Wire.h>
#include <SparkFun_APDS9960.h>
#include <Servo.h>

// Пины для включения питания датчиков
#define SENSOR1_POWER 4  // Питание датчика 1
#define SENSOR2_POWER 5  // Питание датчика 2

// Пины прерываний
#define SENSOR1_INT 2
#define SENSOR2_INT 3

// Пины сервоприводов
#define SERVO_GRAB 7
#define SERVO_EXTEND 8  
#define SERVO_LIFT 9
#define SERVO_TURN 10

byte servoPositions[4] = {90, 90, 90, 90};
SparkFun_APDS9960 apds;
Servo servos[4];

void enableSensor1() {
  digitalWrite(SENSOR1_POWER, HIGH);  // Включить датчик 1
  digitalWrite(SENSOR2_POWER, LOW);   // Выключить датчик 2
  delay(100); // Дать время на стабилизацию питания
}

void enableSensor2() {
  digitalWrite(SENSOR1_POWER, LOW);   // Выключить датчик 1
  digitalWrite(SENSOR2_POWER, HIGH);  // Включить датчик 2
  delay(100); // Дать время на стабилизацию питания
}

void disableBothSensors() {
  digitalWrite(SENSOR1_POWER, LOW);
  digitalWrite(SENSOR2_POWER, LOW);
}

void setup() {
  Serial.begin(9600);
  Serial.println("Запуск системы с раздельным питанием датчиков...");
  
  // Настройка пинов питания датчиков
  pinMode(SENSOR1_POWER, OUTPUT);
  pinMode(SENSOR2_POWER, OUTPUT);
  pinMode(SENSOR1_INT, INPUT);
  pinMode(SENSOR2_INT, INPUT);
  
  // Отключаем оба датчика сначала
  disableBothSensors();
  
  // Инициализация сервоприводов
  servos[0].attach(SERVO_GRAB);
  servos[1].attach(SERVO_EXTEND);
  servos[2].attach(SERVO_LIFT);
  servos[3].attach(SERVO_TURN);
  
  // Установка начальных позиций
  for (int i = 0; i < 4; i++) {
    servos[i].write(servoPositions[i]);
  }
  
  // Инициализация I2C
  Wire.begin();
  delay(100);
  
  // Тест I2C шины
  testI2C();
  
  // Инициализация датчика 1
  Serial.println("Инициализация датчика 1...");
  enableSensor1();
  if (apds.init()) {
    apds.enableGestureSensor(true);
    apds.setGestureLEDDrive(3);
    Serial.println("Датчик 1 OK");
  } else {
    Serial.println("ОШИБКА: Датчик 1 не инициализирован!");
  }
  
  // Инициализация датчика 2
  Serial.println("Инициализация датчика 2...");
  enableSensor2();
  if (apds.init()) {
    apds.enableGestureSensor(true);
    apds.setGestureLEDDrive(3);
    Serial.println("Датчик 2 OK");
  } else {
    Serial.println("ОШИБКА: Датчик 2 не инициализирован!");
  }
  
  // Отключаем оба датчика после инициализации
  disableBothSensors();
  
  Serial.println("Система готова");
}

void loop() {
  // Обработка датчика 1
  if (digitalRead(SENSOR1_INT) == LOW) {
    handleSensor(1);
  }
  
  // Обработка датчика 2
  if (digitalRead(SENSOR2_INT) == LOW) {
    handleSensor(2);
  }
  
  delay(50);
}

void handleSensor(int sensorNum) {
  Serial.print(">>> Прерывание от ДАТЧИКА ");
  Serial.println(sensorNum);
  
  // Включаем нужный датчик
  if (sensorNum == 1) {
    enableSensor1();
  } else {
    enableSensor2();
  }
  
  delay(50); // Даем время на инициализацию
  
  // Читаем жест с повторными попытками
  int gesture = 0;
  for (int i = 0; i < 5; i++) {
    gesture = apds.readGesture();
    if (gesture != 0 && gesture != 255) {
      break; // Успешно прочитали жест
    }
    delay(10);
  }
  
  if (gesture == 255 || gesture == 0) {
    Serial.println("Ошибка чтения жеста или нет жеста");
  } else {
    Serial.print("Д");
    Serial.print(sensorNum);
    Serial.print(" жест: ");
    Serial.println(gesture);
    
    // Обработка жестов
    processGesture(sensorNum, gesture);
  }
  
  // Отключаем датчики
  disableBothSensors();
}

void processGesture(int sensorNum, int gesture) {
  switch (gesture) {
    case DIR_UP:
      if (sensorNum == 1) {
        servoPositions[3] = constrain(servoPositions[3] - 15, 0, 180);
        servos[3].write(servoPositions[3]);
        Serial.println("Поворот ВЛЕВО");
      } else {
        servoPositions[2] = constrain(servoPositions[2] + 15, 0, 180);
        servos[2].write(servoPositions[2]);
        Serial.println("Поднять ВВЕРХ");
      }
      break;
      
    case DIR_DOWN:
      if (sensorNum == 1) {
        servoPositions[3] = constrain(servoPositions[3] + 15, 0, 180);
        servos[3].write(servoPositions[3]);
        Serial.println("Поворот ВПРАВО");
      } else {
        servoPositions[2] = constrain(servoPositions[2] - 15, 0, 180);
        servos[2].write(servoPositions[2]);
        Serial.println("Опустить ВНИЗ");
      }
      break;
      
    case DIR_LEFT:
      if (sensorNum == 1) {
        servoPositions[1] = constrain(servoPositions[1] + 15, 0, 180);
        servos[1].write(servoPositions[1]);
        Serial.println("Вперед");
      } else {
        servoPositions[0] = constrain(servoPositions[0] - 15, 0, 180);
        servos[0].write(servoPositions[0]);
        Serial.println("Сжать");
      }
      break;
      
    case DIR_RIGHT:
      if (sensorNum == 1) {
        servoPositions[1] = constrain(servoPositions[1] - 15, 0, 180);
        servos[1].write(servoPositions[1]);
        Serial.println("Назад");
      } else {
        servoPositions[0] = constrain(servoPositions[0] + 15, 0, 180);
        servos[0].write(servoPositions[0]);
        Serial.println("Разжать");
      }
      break;
  }
}

void testI2C() {
  Serial.println("=== СКАНИРОВАНИЕ I2C ===");
  byte error, address;
  int nDevices = 0;
  
  for(address = 1; address < 127; address++) {
    Wire.beginTransmission(address);
    error = Wire.endTransmission();
    
    if (error == 0) {
      Serial.print("Устройство I2C найдено по адресу 0x");
      if (address < 16) Serial.print("0");
      Serial.println(address, HEX);
      nDevices++;
    }
  }
  
  if (nDevices == 0) {
    Serial.println("Устройства I2C не найдены!");
  } else {
    Serial.println("Сканирование завершено");
  }
  Serial.println("=======================");
}