---

#include <Wire.h>

// --- I2C Адреса устройств ---
const uint8_t ACCEL_ADDR = 0x68;  // Адрес MPU6050
const uint8_t LIGHT_ADDR = 0x50;  // Адрес планки S01.0 (IntroSat)

// --- Пины управления маховиком ---
const int MOTOR_PWM_PIN = PA1;    // ШИМ скорость (0-255)
const int MOTOR_DIR_PIN = PA2;    // Направление вращения

// --- Пороги срабатывания ---
const float ACCEL_THRESHOLD = 1.5;   // Порог ускорения в g (покой ~1.0g)
const uint16_t LIGHT_THRESHOLD = 5000; // Порог света (настройте под свои значения)

void setup() {
  // Настройка порта отладки (Serial1 для UART PA9/PA10 или Serial для USB CDC)
  Serial1.begin(115200);
  Wire.begin();

  // Настройка пинов маховика
  pinMode(MOTOR_PWM_PIN, OUTPUT);
  pinMode(MOTOR_DIR_PIN, OUTPUT);

  // Пробуждаем акселерометр
  Wire.beginTransmission(ACCEL_ADDR);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  Serial1.println("Система CubeSat запущенa. Опрос датчиков 0x68 и 0x50...");
}

void loop() {
  // ==================== 1. ЧТЕНИЕ АКСЕЛЕРОМЕТРА (0x68) ====================
  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;

  // Модуль суммарного ускорения: A = sqrt(ax^2 + ay^2 + az^2)
  float totalAccel = sqrt(ax * ax + ay * ay + az * az);

  // ==================== 2. ЧТЕНИЕ ОСВЕЩЕННОСТИ (0x50) ====================
  uint16_t lightValue = 0;
  
  Wire.beginTransmission(LIGHT_ADDR);
  Wire.write(0x00); // Указываем регистра данных
  Wire.endTransmission(false);
  Wire.requestFrom((uint8_t)LIGHT_ADDR, (uint8_t)2, (uint8_t)true);

  if (Wire.available() >= 2) {
    uint8_t highByte = Wire.read();
    uint8_t lowByte = Wire.read();
    lightValue = (highByte << 8) | lowByte;
  }

  // ==================== ВЫВОД ДАННЫХ В КОНСОЛЬ ====================
  Serial1.print("Ускорение: "); Serial1.print(totalAccel, 2); Serial1.print(" g | ");
  Serial1.print("Свет (0x50): "); Serial1.print(lightValue);

  // ==================== 3. ЛОГИКА УПРАВЛЕНИЯ МАХОВИКОМ ====================
  if (totalAccel > ACCEL_THRESHOLD) {
    // Включение при превышении ускорения (стабилизация)
    digitalWrite(MOTOR_DIR_PIN, HIGH);
    analogWrite(MOTOR_PWM_PIN, 220);
    Serial1.println(" -> [МАХОВИК] Включен: Стабилизация");
  } 
  else if (lightValue < LIGHT_THRESHOLD) {
    // Включение при низкой освещенности (поворот к солнцу)
    digitalWrite(MOTOR_DIR_PIN, LOW);
    analogWrite(MOTOR_PWM_PIN, 120);
    Serial1.println(" -> [МАХОВИК] Включен: Поиск света");
  } 
  else {
    // Отключение маховика
    analogWrite(MOTOR_PWM_PIN, 0);
    Serial1.println(" -> [МАХОВИК] Выключен");
  }

  delay(200);
}