# === MAIN MOTOR CONTROL (PWM) ===
# Controls the brushed pusher motor via PWM
from machine import Pin, PWM
import time

motor_pin = PWM(Pin(5))  # Replace 5 with actual motor pin
motor_pin.freq(1000)

def set_throttle(percent):
    duty = int((percent / 100.0) * 1023)
    motor_pin.duty(duty)

# Example: set_throttle(50) for 50% power

# === ACTUATOR CONTROL (WITH DRV8833) ===
# Controls two coil actuators for stabilizer or rudder control
IN1 = Pin(12, Pin.OUT)
IN2 = Pin(13, Pin.OUT)
IN3 = Pin(14, Pin.OUT)
IN4 = Pin(15, Pin.OUT)

def actuator1(direction):
    if direction == "left":
        IN1.on(); IN2.off()
    elif direction == "right":
        IN1.off(); IN2.on()
    else:
        IN1.off(); IN2.off()

def actuator2(direction):
    if direction == "up":
        IN3.on(); IN4.off()
    elif direction == "down":
        IN3.off(); IN4.on()
    else:
        IN3.off(); IN4.off()

# === COMMUNICATION RECEIVER (ESP-NOW) ===
# Receives control packets from transmitter
import espnow
import network
import struct

w0 = network.WLAN(network.STA_IF)
w0.active(True)
e = espnow.ESPNow()
e.init()
e.add_peer(b'\xFF\xFF\xFF\xFF\xFF\xFF')  # broadcast

while True:
    host, msg = e.recv()
    if msg:
        throttle, a1_dir, a2_dir = struct.unpack('B2s2s', msg)
        set_throttle(throttle)
        actuator1(a1_dir.decode())
        actuator2(a2_dir.decode())

# === FPV STREAMING (ESP32-CAM) ===
# Streams video over WiFi using MJPEG HTTP server
# Use Arduino IDE with ESP32-CAM board selected

#include "esp_camera.h"
#include <WiFi.h>
#include "camera_pins.h"

const char* ssid = "YourSSID";
const char* password = "YourPassword";

void startCameraServer();

void setup() {
  Serial.begin(115200);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  camera_config_t config;
  config.ledc_channel = LEDC_CHANNEL_0;
  config.ledc_timer = LEDC_TIMER_0;
  config.pin_d0 = Y2_GPIO_NUM;
  config.pin_d1 = Y3_GPIO_NUM;
  config.pin_d2 = Y4_GPIO_NUM;
  config.pin_d3 = Y5_GPIO_NUM;
  config.pin_d4 = Y6_GPIO_NUM;
  config.pin_d5 = Y7_GPIO_NUM;
  config.pin_d6 = Y8_GPIO_NUM;
  config.pin_d7 = Y9_GPIO_NUM;
  config.pin_xclk = XCLK_GPIO_NUM;
  config.pin_pclk = PCLK_GPIO_NUM;
  config.pin_vsync = VSYNC_GPIO_NUM;
  config.pin_href = HREF_GPIO_NUM;
  config.pin_sscb_sda = SIOD_GPIO_NUM;
  config.pin_sscb_scl = SIOC_GPIO_NUM;
  config.pin_pwdn = PWDN_GPIO_NUM;
  config.pin_reset = RESET_GPIO_NUM;
  config.xclk_freq_hz = 20000000;
  config.pixel_format = PIXFORMAT_JPEG;
  config.frame_size = FRAMESIZE_QVGA;
  config.jpeg_quality = 10;
  config.fb_count = 2;

  // Init camera
  esp_err_t err = esp_camera_init(&config);
  if (err != ESP_OK) {
    Serial.printf("Camera init failed with error 0x%x", err);
    return;
  }

  startCameraServer();
  Serial.println("Camera ready!");
}

#include <WebServer.h>
WebServer server(80);

void handleJPGStream() {
  WiFiClient client = server.client();
  String response = "HTTP/1.1 200 OK\r\n";
  response += "Content-Type: multipart/x-mixed-replace; boundary=frame\r\n\r\n";
  server.sendContent(response);

  while (1) {
    camera_fb_t * fb = esp_camera_fb_get();
    if (!fb) {
      Serial.println("Camera capture failed");
      return;
    }
    response = "--frame\r\n";
    response += "Content-Type: image/jpeg\r\n\r\n";
    server.sendContent(response);
    client.write(fb->buf, fb->len);
    server.sendContent("\r\n");
    esp_camera_fb_return(fb);
  }
}

void startCameraServer() {
  server.on("/", HTTP_GET, []() {
    server.sendHeader("Location", "/stream");
    server.send(302, "text/plain", "");
  });
  server.on("/stream", HTTP_GET, handleJPGStream);
  server.begin();
}

// ===========================
// 📦 FILE: fpv_drone_full_code.py
// ===========================

# This single Python-like pseudocode file includes all ESP32-side code (Arduino framework), divided into:
# 1. ESP32-CAM (Video + Overlay + Web UI)
# 2. ESP32 Master (Flight Control, Sensors, Motors)
# Communication between both is via UART or Wi-Fi as needed.

// ---------------------------------------------------
// 1. ESP32-CAM CODE (CAMERA + TELEMETRY OVERLAY + CONTROL UI)
// ---------------------------------------------------
// Arduino code - use in Arduino IDE

#include "esp_camera.h"
#include <WiFi.h>
#include <WebServer.h>

const char* ssid = "DRONE-FPV";
const char* password = "12345678";

WebServer server(80);

// Sample telemetry variables (to be filled via UART or WiFi)
String battery = "3.7V";
String altitude = "12.5m";
String gps = "51.5074,-0.1278";

void startCameraServer();

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

  WiFi.softAP(ssid, password);
  delay(100);
  IPAddress IP = WiFi.softAPIP();
  Serial.println("AP IP address: ");
  Serial.println(IP);

  camera_config_t config;
  // (camera config omitted for brevity, use AI Thinker board definition)

  // Start camera
  esp_err_t err = esp_camera_init(&config);
  if (err != ESP_OK) {
    Serial.printf("Camera init failed: 0x%x", err);
    return;
  }

  startCameraServer();
  server.begin();
  Serial.println("HTTP server started");
}

void loop() {
  server.handleClient();
}

// Example camera server with MJPEG stream
void startCameraServer() {
  server.on("/", HTTP_GET, []() {
    server.send(200, "text/html", "<html><body><h1>Drone FPV</h1><img src='/stream'><br>Battery: " + battery + "<br>Alt: " + altitude + "<br>GPS: " + gps + "</body></html>");
  });
  server.on("/stream", HTTP_GET, []() {
    WiFiClient client = server.client();
    String response = "HTTP/1.1 200 OK\r\n";
    response += "Content-Type: multipart/x-mixed-replace; boundary=frame\r\n\r\n";
    server.sendContent(response);

    while (client.connected()) {
      camera_fb_t * fb = esp_camera_fb_get();
      if (!fb) {
        Serial.println("Camera capture failed");
        return;
      }
      server.sendContent("--frame\r\nContent-Type: image/jpeg\r\nContent-Length: " + String(fb->len) + "\r\n\r\n");
      client.write(fb->buf, fb->len);
      server.sendContent("\r\n");
      esp_camera_fb_return(fb);
      delay(30);
    }
  });
}

// ---------------------------------------------------
// 2. ESP32 MASTER CODE (MOTOR + SENSORS + TELEMETRY)
// ---------------------------------------------------
// Arduino code - use in Arduino IDE

#include <WiFi.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

Adafruit_MPU6050 mpu;

int motor1Pin1 = 13;
int motor1Pin2 = 12;
int ENA = 14; // PWM control to DRV8833

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

  pinMode(motor1Pin1, OUTPUT);
  pinMode(motor1Pin2, OUTPUT);
  pinMode(ENA, OUTPUT);

  // Wi-Fi connection to ESP32-CAM AP
  WiFi.begin("DRONE-FPV", "12345678");
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("Connected to ESP32-CAM");
}

void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Example telemetry string
  String telemetry = "Battery:3.7V;Alt:15.0m;GPS:51.5074,-0.1278";

  // Send telemetry to CAM (can also be via UART)
  WiFiClient client;
  if (client.connect(IPAddress(192,168,4,1), 80)) {
    client.print("POST /telemetry HTTP/1.1\r\n");
    client.print("Host: 192.168.4.1\r\n");
    client.print("Content-Length: ");
    client.print(telemetry.length());
    client.print("\r\n\r\n");
    client.print(telemetry);
    client.stop();
  }

  // Example motor control logic
  analogWrite(ENA, 150);
  digitalWrite(motor1Pin1, HIGH);
  digitalWrite(motor1Pin2, LOW);

  delay(100);
}

// NOTE: You may split these into separate .ino files for ESP32-CAM and ESP32 main board.
// Each block works independently and communicates over WiFi AP.

// For more advanced control, add UDP/WebSocket, PID logic, or Bluetooth gamepad control.