#include <SPI.h>
#include <LoRa.h>

#define SS 5
#define RST 14
#define DIO0 26

#define VRx 34 // Roll/Pitch
#define VRy 35 // Throttle/Yaw

void setup() {
  Serial.begin(115200);
  while(!Serial);

  // LoRa init
  LoRa.setPins(SS, RST, DIO0);
  if(!LoRa.begin(433E6)){ // 433MHz
    Serial.println("Starting LoRa failed!");
    while(1);
  }
  Serial.println("LoRa Ground Station ready");
}

void loop() {
  int roll = analogRead(VRx);    // 0-4095
  int throttle = analogRead(VRy); // 0-4095

  // Map to -100 to 100 for easier control
  roll = map(roll, 0, 4095, -100, 100);
  throttle = map(throttle, 0, 4095, 0, 100);

  // Send data
  LoRa.beginPacket();
  LoRa.print(roll);
  LoRa.print(",");
  LoRa.print(throttle);
  LoRa.endPacket();

  delay(50); // send every 50ms
}

#include <SPI.h>
#include <LoRa.h>

#define SS 5
#define RST 14
#define DIO0 26

int roll, throttle;

void setup() {
  Serial.begin(115200);
  while(!Serial);

  LoRa.setPins(SS, RST, DIO0);
  if(!LoRa.begin(433E6)){
    Serial.println("Starting LoRa failed!");
    while(1);
  }
  Serial.println("LoRa Drone Receiver ready");
}

void loop() {
  int packetSize = LoRa.parsePacket();
  if(packetSize){
    String received = "";
    while(LoRa.available()){
      received += (char)LoRa.read();
    }
    Serial.println("Received: " + received);

    int commaIndex = received.indexOf(',');
    if(commaIndex > 0){
      roll = received.substring(0, commaIndex).toInt();
      throttle = received.substring(commaIndex + 1).toInt();

      // TODO: Map roll/throttle to motor ESC PWM signals
      Serial.print("Roll: "); Serial.print(roll);
      Serial.print(" Throttle: "); Serial.println(throttle);
    }
  }
}

#include <Wire.h>
#include <SPI.h>
#include <LoRa.h>
#include <Adafruit_MPU6050.h> // Use MPU6050 library compatible with MPU9260
#include <Adafruit_Sensor.h>
#include "esp_camera.h"

#define SS 5
#define RST 14
#define DIO0 26

// Motor Pins
#define M1 12
#define M2 13
#define M3 14
#define M4 15

Adafruit_MPU6050 mpu;

// LoRa
void setupLoRa() {
  LoRa.setPins(SS, RST, DIO0);
  if (!LoRa.begin(433E6)) {
    Serial.println("LoRa init failed!");
    while(1);
  }
  Serial.println("LoRa ready");
}

// ESC PWM mapping
void setMotorPWM(int m1,int m2,int m3,int m4){
  ledcWrite(0,map(m1,1000,2000,0,255));
  ledcWrite(1,map(m2,1000,2000,0,255));
  ledcWrite(2,map(m3,1000,2000,0,255));
  ledcWrite(3,map(m4,1000,2000,0,255));
}

// Camera
void setupCamera() {
  camera_config_t config;
  config.ledc_channel = LEDC_CHANNEL_0;
  config.ledc_timer   = LEDC_TIMER_0;
  config.pin_d0 = 5;
  config.pin_d1 = 18;
  config.pin_d2 = 19;
  config.pin_d3 = 21;
  config.pin_d4 = 36;
  config.pin_d5 = 39;
  config.pin_d6 = 34;
  config.pin_d7 = 35;
  config.pin_xclk = 0;
  config.pin_pclk = 22;
  config.pin_vsync = 25;
  config.pin_href = 23;
  config.pin_sscb_sda = 26;
  config.pin_sscb_scl = 27;
  config.pin_pwdn = 32;
  config.pin_reset = -1;
  config.xclk_freq_hz = 20000000;
  config.pixel_format = PIXFORMAT_JPEG;
  config.frame_size = FRAMESIZE_QVGA;
  config.jpeg_quality = 10;
  config.fb_count = 1;
  esp_camera_init(&config);
}

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

  // Motor PWM setup
  ledcSetup(0, 400, 8);
  ledcSetup(1, 400, 8);
  ledcSetup(2, 400, 8);
  ledcSetup(3, 400, 8);
  ledcAttachPin(M1,0);
  ledcAttachPin(M2,1);
  ledcAttachPin(M3,2);
  ledcAttachPin(M4,3);

  // MPU
  if (!mpu.begin()) {
    Serial.println("MPU failed");
    while(1);
  }
  Serial.println("MPU ready");

  // LoRa
  setupLoRa();

  // Camera
  setupCamera();
}

void loop() {
  // Read MPU data
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  float roll  = a.acceleration.x;
  float pitch = a.acceleration.y;
  float yaw   = a.acceleration.z;

  // Receive LoRa commands (throttle etc.)
  int throttle=1500;
  int rollInput=0;
  int pitchInput=0;
  int yawInput=0;

  int packetSize = LoRa.parsePacket();
  if(packetSize){
    String data="";
    while(LoRa.available()) data += (char)LoRa.read();
    int comma1=data.indexOf(',');
    int comma2=data.indexOf(',',comma1+1);
    int comma3=data.indexOf(',',comma2+1);
    if(comma1>0 && comma2>0 && comma3>0){
      throttle = data.substring(0,comma1).toInt();
      rollInput = data.substring(comma1+1,comma2).toInt();
      pitchInput= data.substring(comma2+1,comma3).toInt();
      yawInput  = data.substring(comma3+1).toInt();
    }
  }

  // Motor mixing formula (X-quad)
  int m1 = constrain(throttle + pitchInput + rollInput - yawInput,1000,2000);
  int m2 = constrain(throttle + pitchInput - rollInput + yawInput,1000,2000);
  int m3 = constrain(throttle - pitchInput + rollInput + yawInput,1000,2000);
  int m4 = constrain(throttle - pitchInput - rollInput - yawInput,1000,2000);

  setMotorPWM(m1,m2,m3,m4);

  // Optional: capture image every 5 seconds
  static unsigned long lastCapture=0;
  if(millis()-lastCapture>5000){
    camera_fb_t *fb = esp_camera_fb_get();
    if(fb){
      Serial.print("Captured image size: "); Serial.println(fb->len);
      esp_camera_fb_return(fb);
    }
    lastCapture = millis();
  }

  delay(50);
}