#include <Servo.h>
#include <Wire.h>
#include <Adafruit_GPS.h>
#include <SD.h>
#include <SPI.h>
#include <MPU6050_tockn.h>
#include <HMC5883L.h>
#include <BMP085.h>
#include <Kalman.h>
#include <PID_Autotune_v0.h>
#include <PID_v1.h>

// Define pins
const int motor1Pin = 9;
const int motor2Pin = 10;
const int motor3Pin = 11;
const int motor4Pin = 12;
const int gpsPin = 9; // Serial port for GPS module
const int sdCardCS = 10; // Chip Select pin for SD card

// Create objects
Servo motor1;
Servo motor2;
Servo motor3;
Servo motor4;
Adafruit_GPS GPS(&Serial1);
MPU6050 mpu;
HMC5883L compass;
BMP085 barometer;
File dataFile;

// Kalman filter objects
Kalman kalmanX; // Create Kalman filter objects for each axis
Kalman kalmanY;
Kalman kalmanZ;

// PID controllers
PID myPID_roll;
PID myPID_pitch;
PID myPID_yaw;
PID myPID_alt;

// Sensor data variables
float accelX, accelY, accelZ, gyroX, gyroY, gyroZ;
float magX, magY, magZ;
float altitude, groundSpeed, course;
float targetLat, targetLon; // Target coordinates
float roll, pitch, yaw;
float filteredRoll, filteredPitch, filteredYaw;

// Flight mode
enum FlightMode { MANUAL, STABILIZE, ALTITUDE_HOLD, GPS_HOLD, MISSION, LANDING };
FlightMode flightMode = MANUAL;

// Motor speed variables
int baseSpeed = 1500; // Hovering speed
int motor1Speed, motor2Speed, motor3Speed, motor4Speed;

// GPS variables
float currentLat, currentLon;

// Altitude hold variables
float targetAltitude = 50.0; // Set target altitude in meters

// Navigation variables
float targetBearing;
float distanceToTarget;
float navRoll, navPitch; // Navigation roll and pitch commands

// PID control outputs
double rollOutput, pitchOutput, yawOutput, altOutput;

// Autotune variables
double Output;
unsigned int AutotuneComplete = 0;

// Loop timing
unsigned long previousTime = 0;
const unsigned long loopInterval = 10; // Loop interval in milliseconds

void setup() {
  // Initialize serial communication
  Serial.begin(9600);
  Serial1.begin(9600); // For GPS module

  // Initialize SD card
  if (!SD.begin(sdCardCS)) {
    Serial.println("SD card initialization failed!");
    while (1);
  }

  // Attach servos to pins
  motor1.attach(motor1Pin);
  motor2.attach(motor2Pin);
  motor3.attach(motor3Pin);
  motor4.attach(motor4Pin);

  // Initialize I2C communication
  Wire.begin();

  // Initialize MPU6050 sensor
  if (!mpu.begin()) {
    Serial.println("MPU6050 initialization failed!");
    while (1);
  }

  // Initialize HMC5883L compass
  if (!compass.begin()) {
    Serial.println("HMC5883L initialization failed!");
    while (1);
  }

  // Initialize BMP085 barometer
  if (!barometer.begin()) {
    Serial.println("BMP085 initialization failed!");
    while (1);
  }

  // Initialize GPS module
  GPS.begin();

  // Initialize Kalman filter
  kalmanX.setQ(0.001); // Process noise covariance
  kalmanX.setR(0.03); // Measurement noise covariance
  kalmanY.setQ(0.001);
  kalmanY.setR(0.03);
  kalmanZ.setQ(0.001);
  kalmanZ.setR(0.03);

  // Initialize PID controllers
  myPID_roll.SetMode(AUTOMATIC);
  myPID_pitch.SetMode(AUTOMATIC);
  myPID_yaw.SetMode(AUTOMATIC);
  myPID_alt.SetMode(AUTOMATIC);

  // Set initial motor speeds (hovering speed)
  setMotorSpeeds();

  // Start autotune for roll PID (example)
  myPID_roll.SetTunings(0.3, 0, 0); // Initial guess for PID gains
  myPID_roll.SetSampleTime(10); // Set sample time in milliseconds
  myPID_roll.SetOutputLimits(-200, 200); // Set output limits
  myPID_roll.SetMode(AUTOMATIC);
}

void loop() {
  unsigned long currentTime = millis();
  if (currentTime - previousTime >= loopInterval) {
    previousTime = currentTime;

    readSensorData();
    calculateAttitude(); 
    handleGPS();

    // Kalman filter for sensor fusion
    filteredRoll = kalmanX.getAngle(roll, gyroX, 0.01); // Assuming 10ms loop time
    filteredPitch = kalmanY.getAngle(pitch, gyroY, 0.01);
    filteredYaw = kalmanZ.getAngle(yaw, gyroZ, 0.01);

    // Calculate PID control signals
    myPID_roll.Compute(filteredRoll, 0, &rollOutput); // Setpoint for roll is 0
    myPID_pitch.Compute(filteredPitch, 0, &pitchOutput);
    myPID_yaw.Compute(filteredYaw, 0, &yawOutput);

    // Altitude hold control
    if (flightMode == ALTITUDE_HOLD) {
      // Calculate altitude error
      float altitudeError = targetAltitude - altitude; 
      // Calculate altitude control signal (adjust based on altitude error)
      altOutput = kp_alt * altitudeError; 
    } else {
      altOutput = 0;
    }

    // GPS navigation control
    if (flightMode == GPS_HOLD || flightMode == MISSION) {
      // Calculate distance and bearing to target
      distanceToTarget = calculateDistance(currentLat, currentLon, targetLat, targetLon); 
      targetBearing = calculateBearing(currentLat, currentLon, targetLat, targetLon); 
      // Calculate navigation roll and pitch commands
      // ... (Implement navigation logic)
    } else {
      navRoll = 0;
      navPitch = 0;
    }

    // Combine control signals
    rollOutput += navRoll;
    pitchOutput += navPitch;

    // Calculate motor speeds based on control signals
    motor1Speed = baseSpeed + rollOutput - pitchOutput - yawOutput + altOutput;
    motor2Speed = baseSpeed - rollOutput - pitchOutput + yawOutput + altOutput;
    motor3Speed = baseSpeed - rollOutput + pitchOutput - yawOutput + altOutput;
    motor4Speed = baseSpeed + rollOutput + pitchOutput + yawOutput + altOutput;

    // Set motor speeds
    setMotorSpeeds();

    // Arm/disarm control
    armDisarm();

    // Read and write to SD card (optional)
    readSDCard();
    writeSDCard();

    // Print sensor data and control signals (optional)
    // ...

  }
}

// Sensor data acquisition functions (implementations)
void readSensorData() {
  // Read accelerometer and gyroscope data from MPU6050
  mpu.getMotion6(&accelX, &accelY, &accelZ, &gyroX, &gyroY, &gyroZ);

  // Read magnetometer data from HMC5883L
  compass.read();
  magX = compass.getX();
  magY = compass.getY();
  magZ = compass.getZ();

  // Read altitude data from BMP085
  altitude = barometer.readAltitude();
}

// Attitude calculation function
void calculateAttitude() {
  // Calculate roll, pitch, yaw angles using accelerometer and gyroscope data
  // Implement appropriate algorithms (e.g., complementary filter, Madgwick filter)
  // ...
}

// GPS data handling function
void handleGPS() {
  while (GPS.available()) {
    char c = GPS.read();
    GPS.encode(c);
  }

  if (GPS.fix) {
    currentLat = GPS.latitudeDegrees;
    currentLon = GPS.longitudeDegrees;
    groundSpeed = GPS.speed;
    course = GPS.course;
  }
}

// Calculate distance between two GPS