---

#include <Wire.h>

// --- I2C Адреса устройств ---
const uint8_t ACCEL_ADDR    = 0x68; // Адрес акселерометра MPU6050
const uint8_t FLYWHEEL_ADDR = 0x60; // Адрес Flywheel Module (уточните по I2C сканеру)

// Адреса 4 планок освещенности S01.0 (задаются DIP-переключателями SW1: 0x50, 0x51, 0x52, 0x53)
const uint8_t LIGHT_ADDRS[4] = {0x50, 0x51, 0x52, 0x53};

// --- Пороги срабатывания ---
const float ACCEL_THRESHOLD    = 1.5;   // Порог ускорения в g
const uint16_t LIGHT_THRESHOLD = 3000;  // Порог освещенности для ориентации

// Чтение данных с планки S01.0
uint16_t readLightSensor(uint8_t addr) {
  uint16_t value = 0;
  Wire.beginTransmission(addr);
  Wire.write(0x00);
  if (Wire.endTransmission(false) == 0) {
    Wire.requestFrom((uint8_t)addr, (uint8_t)2, (uint8_t)true);
    if (Wire.available() >= 2) {
      value = (Wire.read() << 8) | Wire.read();
    }
  }
  return value;
}

// Передача заданной скорости на модуль маховика по I2C
void setFlywheelSpeed(int8_t speed) {
  Wire.beginTransmission(FLYWHEEL_ADDR);
  Wire.write(speed); // Передача значения скорости (-100...100 или 0...255)
  Wire.endTransmission();
}

void setup() {
  // Настройка порта отладки (Serial1 для UART PA9/PA10 или Serial для USB CDC)
  Serial1.begin(115200);
  Wire.begin(); // Инициализация I2C1 (PB6 - SCL, PB7 - SDA)

  // Инициализация / Пробуждение MPU6050
  Wire.beginTransmission(ACCEL_ADDR);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  Serial1.println("Система CubeSat запущенa: 4 датчика света + акселерометр + маховик.");
}

void loop() {
  // ==================== 1. ЧТЕНИЕ АКСЕЛЕРОМЕТРА ====================
  int16_t rawX, rawY, rawZ;
  Wire.beginTransmission(ACCEL_ADDR);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom((uint8_t)ACCEL_ADDR, (uint8_t)6, (uint8_t)true);

  rawX = (Wire.read() << 8) | Wire.read();
  rawY = (Wire.read() << 8) | Wire.read();
  rawZ = (Wire.read() << 8) | Wire.read();

  float ax = rawX / 16384.0;
  float ay = rawY / 16384.0;
  float az = rawZ / 16384.0;
  float totalAccel = sqrt(ax * ax + ay * ay + az * az);

  // ==================== 2. ЧТЕНИЕ 4 ДАТЧИКОВ ОСВЕЩЕННОСТИ ====================
  uint16_t lightValues[4];
  uint16_t maxLight = 0;
  uint8_t brightestFace = 0;

  for (int i = 0; i < 4; i++) {
    lightValues[i] = readLightSensor(LIGHT_ADDRS[i]);
    if (lightValues[i] > maxLight) {
      maxLight = lightValues[i];
      brightestFace = i; // Определение грани с максимальной освещенностью
    }
  }

  // Вывод данных в Монитор порта
  Serial1.print("Ускорение: "); Serial1.print(totalAccel, 2); Serial1.print(" g | ");
  Serial1.print("Свет [0..3]: ");
  for (int i = 0; i < 4; i++) {
    Serial1.print(lightValues[i]);
    if (i < 3) Serial1.print(", ");
  }

  // ==================== 3. ЛОГИКА УПРАВЛЕНИЯ МАХОВИКОМ ====================
  if (totalAccel > ACCEL_THRESHOLD) {
    // Включение маховика для гашения вращения при резком ускорении
    setFlywheelSpeed(80);
    Serial1.println(" -> [МАХОВИК] Стабилизация");
  } 
  else if (maxLight < LIGHT_THRESHOLD) {
    // Вращение для поиска источника света, если все грани в тени
    setFlywheelSpeed(40);
    Serial1.println(" -> [МАХОВИК] Поиск света");
  } 
  else {
    // Остановка маховика при нормальном положении
    setFlywheelSpeed(0);
    Serial1.print(" -> [МАХОВИК] Выкл (Солнце на грани ");
    Serial1.print(brightestFace);
    Serial1.println(")");
  }

  delay(200);
}