#include <Wire.h>

// --------------------- I2C SETUP ----------------------
TwoWire HWire(2, I2C_FAST_MODE);     // I2C on PB10 (SCL), PB11 (SDA)
uint8_t gyro_address = 0x68;

// --------------------- Gyro Variables ------------------
long gyro_x, gyro_y, gyro_z;

// =======================================================
//                     SETUP
// =======================================================
void setup() {
  Serial.begin(57600);
  delay(200);

  Serial.println(F("--- YMFC-32 Clean Flight Controller ---"));
  Serial.println(F("Press 'a' for menu"));
  Serial.println(F("Press 'b' for I2C scan"));
  Serial.println(F("Press 'c' for Gyro Read"));

  HWire.begin();
  delay(100);

  init_mpu();
}

// =======================================================
//                   MAIN LOOP
// =======================================================
void loop() {

  if (Serial.available()) {
    char cmd = Serial.read();

    if (cmd == 'a') print_menu();
    if (cmd == 'b') scan_i2c();
    if (cmd == 'c') read_gyro_live();
  }
}

// =======================================================
//                   MPU6050 INITIALIZATION
// =======================================================
void init_mpu() {
  HWire.beginTransmission(gyro_address);
  HWire.write(0x6B);
  HWire.write(0x00);           // Wake up MPU6050
  HWire.endTransmission();

  HWire.beginTransmission(gyro_address);
  HWire.write(0x1B);
  HWire.write(0x08);           // Gyro ±500 deg/s
  HWire.endTransmission();

  HWire.beginTransmission(gyro_address);
  HWire.write(0x1A);
  HWire.write(0x03);           // DLPF 43Hz
  HWire.endTransmission();

  Serial.println(F("MPU6050 Initialized."));
}

// =======================================================
//                   I2C SCANNER
// =======================================================
void scan_i2c() {
  Serial.println(F("Scanning I2C..."));

  byte count = 0;

  for (byte i = 1; i < 127; i++) {
    HWire.beginTransmission(i);
    if (HWire.endTransmission() == 0) {
      Serial.print(F("Found device at 0x"));
      Serial.println(i, HEX);
      count++;
    }
  }

  if (count == 0) Serial.println(F("No I2C devices found!"));
  Serial.println();
}

// =======================================================
//                   READ GYRO RAW
// =======================================================
void read_gyro_values() {

  HWire.beginTransmission(gyro_address);
  HWire.write(0x43);
  HWire.endTransmission();

  HWire.requestFrom(gyro_address, 6);

  gyro_x = (HWire.read() << 8) | HWire.read();
  gyro_y = (HWire.read() << 8) | HWire.read();
  gyro_z = (HWire.read() << 8) | HWire.read();
}

// live stream
void read_gyro_live() {
  Serial.println(F("Reading Gyro... press any key to stop"));

  while (!Serial.available()) {
    read_gyro_values();

    Serial.print("GX: "); Serial.print(gyro_x);
    Serial.print(" | GY: "); Serial.print(gyro_y);
    Serial.print(" | GZ: "); Serial.println(gyro_z);

    delay(200);
  }
  Serial.read();  // clear exit key
}

// =======================================================
//                   MENU PRINTER
// =======================================================
void print_menu() {
  Serial.println(F("\n----- YMFC MENU -----"));
  Serial.println(F("a = Show Menu"));
  Serial.println(F("b = I2C Scanner"));
  Serial.println(F("c = Gyro Raw Read"));
  Serial.println(F("---------------------"));
}