#include <Wire.h>
#include <MPU6050.h>

// MPU6050 objektum
MPU6050 mpu;

// IMU adatokat tároló változók
float accX, accY, accZ;
float gyroX, gyroY, gyroZ;
float angleX, angleY;
float dt = 0.01;

// PID szabályozó változók
float kp = 2.0, ki = 0.5, kd = 1.0;
float errorX, errorY, prevErrorX, prevErrorY;
float integralX, integralY, derivativeX, derivativeY;
float pidOutputX, pidOutputY;

// RC bemenetek
int throttle, pitch, roll, yaw;
const int chThrottle = 2; // PWM bemenetek
const int chPitch = 3;
const int chRoll = 4;
const int chYaw = 5;

// Motor PWM értékek
int motor1, motor2, motor3, motor4;
const int motorPin1 = 6, motorPin2 = 9, motorPin3 = 10, motorPin4 = 11;

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

  // MPU6050 inicializálása
  if (!mpu.begin()) {
    Serial.println("IMU not detected!");
    while (1);
  }
  mpu.calcGyroOffsets(true);

  // RC csatornák inicializálása
  pinMode(chThrottle, INPUT);
  pinMode(chPitch, INPUT);
  pinMode(chRoll, INPUT);
  pinMode(chYaw, INPUT);

  // Motor kimenetek inicializálása
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  pinMode(motorPin3, OUTPUT);
  pinMode(motorPin4, OUTPUT);
}

void loop() {
  // RC csatornák olvasása (PWM)
  throttle = pulseIn(chThrottle, HIGH, 25000);
  pitch = pulseIn(chPitch, HIGH, 25000);
  roll = pulseIn(chRoll, HIGH, 25000);
  yaw = pulseIn(chYaw, HIGH, 25000);

  // PWM normalizálása (-500 és 500 között)
  int normPitch = map(pitch, 1000, 2000, -500, 500);
  int normRoll = map(roll, 1000, 2000, -500, 500);
  int normYaw = map(yaw, 1000, 2000, -500, 500);
  int normThrottle = map(throttle, 1000, 2000, 0, 1000);

  // IMU adatok olvasása
  mpu.update();
  accX = mpu.getAccX();
  accY = mpu.getAccY();
  accZ = mpu.getAccZ();
  gyroX = mpu.getGyroX();
  gyroY = mpu.getGyroY();

  // Szögek számítása (komplementer szűrő)
  angleX = 0.98 * (angleX + gyroX * dt) + 0.02 * atan2(accY, accZ) * 180 / PI;
  angleY = 0.98 * (angleY + gyroY * dt) + 0.02 * atan2(accX, accZ) * 180 / PI;

  // PID szabályozó
  errorX = normPitch - angleX; // Távirányító adatai beépítve
  integralX += errorX * dt;
  derivativeX = (errorX - prevErrorX) / dt;
  pidOutputX = kp * errorX + ki * integralX + kd * derivativeX;
  prevErrorX = errorX;

  errorY = normRoll - angleY;
  integralY += errorY * dt;
  derivativeY = (errorY - prevErrorY) / dt;
  pidOutputY = kp * errorY + ki * integralY + kd * derivativeY;
  prevErrorY = errorY;

  // Motor PWM számítása
  motor1 = constrain(normThrottle + pidOutputX + pidOutputY + normYaw, 1200, 1800);
  motor2 = constrain(normThrottle - pidOutputX + pidOutputY - normYaw, 1200, 1800);
  motor3 = constrain(normThrottle + pidOutputX - pidOutputY - normYaw, 1200, 1800);
  motor4 = constrain(normThrottle - pidOutputX - pidOutputY + normYaw, 1200, 1800);

  // Motorok PWM kimenete
  analogWrite(motorPin1, motor1);
  analogWrite(motorPin2, motor2);
  analogWrite(motorPin3, motor3);
  analogWrite(motorPin4, motor4);

  delay(10); // Ciklusidő
}