#include <Arduino.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <ESP32Servo.h>
#include <Wire.h>

// Shared variables between cores (use volatile for safety)
volatile float pitch, roll;
volatile int throttle = 1100; // Default, low throttle for safety
volatile int commandYaw = 0, commandPitch = 0, commandRoll = 0;

// Setup peripherals
Adafruit_MPU6050 mpu;
Servo esc1, esc2, esc3, esc4;

void setup() {
  Serial.begin(115200);
  Wire.begin();

  // Initialize IMU
  if (!mpu.begin()) {
    Serial.println("MPU6050 not found!");
    while(1);
  }

  // Attach ESCs
  esc1.attach(4);
  esc2.attach(5);
  esc3.attach(6);
  esc4.attach(7);

  // Create tasks on each core
  xTaskCreatePinnedToCore(
    stabilizationTask,    // Function to run
    "StabilizationTask",  // Name
    10000,                // Stack size (bytes)
    NULL,                 // Parameter
    1,                    // Priority (1 = higher than command task)
    NULL,                 // Task handle
    0                     // Core number (0 for stabilization)
  );

  xTaskCreatePinnedToCore(
    commandTask,          // Function to run
    "CommandTask",        // Name
    10000,                // Stack size (bytes)
    NULL,                 // Parameter
    0,                    // Priority (0 = lower than stabilization)
    NULL,                 // Task handle
    1                     // Core number (1 for command)
  );
}

void loop() {} // Not used in FreeRTOS tasks

// Stabilization task (Core 0)
void stabilizationTask(void *pvParameters) {
  sensors_event_t a, g, temp;
  float dt, last_time = micros();
  while (1) {
    mpu.getEvent(&a, &g, &temp);
    dt = (micros() - last_time) * 1e-6;
    last_time = micros();

    // Calculate pitch and roll (simplified)
    pitch = atan2(a.acceleration.y, sqrt(a.acceleration.x*a.acceleration.x + a.acceleration.z*a.acceleration.z));
    roll = atan2(-a.acceleration.x, a.acceleration.z);

    // PID calculation (simplified)
    // ... Add your PID code here ...

    // Motor mixing (simplified)
    int m1 = throttle; // Example, adjust for your mixing logic
    int m2 = throttle;
    int m3 = throttle;
    int m4 = throttle;

    // Constrain and write to ESCs
    esc1.writeMicroseconds(constrain(m1, 1100, 1900));
    esc2.writeMicroseconds(constrain(m2, 1100, 1900));
    esc3.writeMicroseconds(constrain(m3, 1100, 1900));
    esc4.writeMicroseconds(constrain(m4, 1100, 1900));

    vTaskDelay(1); // Yield to other tasks
  }
}

// Command task (Core 1)
void commandTask(void *pvParameters) {
  while (1) {
    // Example: Read commands from serial or WiFi
    if (Serial.available()) {
      String cmd = Serial.readStringUntil('\n');
      // Parse command and update throttle, yaw, pitch, roll
      // ... Add your command parsing code here ...
    }

    // Example: If no command, maintain hover (throttle = hover value)
    // ... Add your logic for autonomous hover here ...

    vTaskDelay(10); // Adjust as needed for command rate
  }
}