/* * MECRO KIT - PROYECTO 17: MANDO DE VUELO + TELEMETRÍA OLED
 * Objetivo: Visualización de Setpoint vs Realidad y Salida de Motores
 * Hardware: ESP32, Joystick, MPU6050, OLED SSD1306
 * Autor: MECRO Engineering Dept.
 */

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// --- CONFIGURACIÓN OLED ---
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1 
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

Adafruit_MPU6050 mpu;

// --- 1. DEFINICIONES DE PINES ---
const int PIN_ROLL_RC  = 35; 
const int PIN_PITCH_RC = 34; 

// --- 2. VARIABLES DE CONTROL ---
int rc_roll = 1500;
int rc_pitch = 1500;
int rc_throttle = 1000; 

// Variables PID (Solo P)
float kp = 2.0; 
float error_roll = 0;
float error_pitch = 0;
float angulo_roll_real = 0;
float angulo_pitch_real = 0;
float setpoint_roll = 0;
float setpoint_pitch = 0;

// Variables Motores
int m1, m2, m3, m4;

// Timer para pantalla (Multitasking básico)
unsigned long last_screen_update = 0;

void setup() {
  Serial.begin(115200);
  
  pinMode(PIN_ROLL_RC, INPUT);
  pinMode(PIN_PITCH_RC, INPUT);

  // Iniciar OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("Fallo OLED - Check conexiones"));
    for(;;);
  }
  display.clearDisplay();
  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(0,0);
  display.println("MECRO SYSTEMS");
  display.println("Inicializando IMU...");
  display.display();

  // Iniciar IMU
  if (!mpu.begin()) {
    Serial.println("Fallo MPU6050");
    display.println("Fallo MPU6050");
    display.display();
    while (1) { delay(10); }
  }
  
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ); 
  
  delay(1000);
}

void loop() {
  // --- CICLO DE CONTROL (RÁPIDO) ---
  
  // 1. LEER INPUT
  rc_roll = map(analogRead(PIN_ROLL_RC), 0, 4095, 1000, 2000);
  rc_pitch = map(analogRead(PIN_PITCH_RC), 0, 4095, 1000, 2000);
  
  // 2. LEER SENSORES
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  
  // Conversión simplificada (G a Grados)
  angulo_roll_real = a.acceleration.y * 5.0; 
  angulo_pitch_real = a.acceleration.x * 5.0;

  // 3. CALCULAR ERROR
  setpoint_roll = map(rc_roll, 1000, 2000, -45, 45);
  setpoint_pitch = map(rc_pitch, 1000, 2000, -45, 45);
  
  error_roll = setpoint_roll - angulo_roll_real;
  error_pitch = setpoint_pitch - angulo_pitch_real;
  
  // 4. PID (P)
  float pid_roll = error_roll * kp;
  float pid_pitch = error_pitch * kp;
  
  // 5. MIXER
  rc_throttle = 1300; // Simulación de vuelo estacionario
  
  m1 = constrain(rc_throttle + pid_pitch + pid_roll, 1000, 2000); // TR
  m2 = constrain(rc_throttle + pid_pitch - pid_roll, 1000, 2000); // DR
  m3 = constrain(rc_throttle - pid_pitch - pid_roll, 1000, 2000); // TI
  m4 = constrain(rc_throttle - pid_pitch + pid_roll, 1000, 2000); // DI
  
  // --- CICLO DE TELEMETRÍA (LENTO - 10Hz) ---
  // Actualizamos la pantalla cada 100ms para no bloquear el loop de vuelo
  if (millis() - last_screen_update > 100) {
    updateOLED();
    last_screen_update = millis();
  }
  
  delay(10); // Pequeña espera para estabilidad en simulación
}

// Función auxiliar para dibujar en OLED
void updateOLED() {
  display.clearDisplay();
  
  // Cabecera
  display.setCursor(0,0);
  display.print("MECRO FC  Bat:OK");
  
  // Mostrar Ángulos: Objetivo (S) vs Real (R)
  display.setCursor(0, 15);
  display.print("RLL S:"); display.print((int)setpoint_roll);
  display.print(" R:"); display.println((int)angulo_roll_real);
  
  display.setCursor(0, 25);
  display.print("PCH S:"); display.print((int)setpoint_pitch);
  display.print(" R:"); display.println((int)angulo_pitch_real);

  // Visualización de Motores (Barras simples)
  // Mapeamos 1000-2000 a 0-30 píxeles de ancho
  int w1 = map(m1, 1000, 2000, 0, 30);
  int w2 = map(m2, 1000, 2000, 0, 30);
  int w3 = map(m3, 1000, 2000, 0, 30);
  int w4 = map(m4, 1000, 2000, 0, 30);

  // Dibujar barras de potencia
  // Arriba: Motores Delanteros (M4, M2)
  display.fillRect(0, 40, w4, 4, WHITE);  display.setCursor(35, 40); display.print("M4");
  display.fillRect(64, 40, w2, 4, WHITE); display.setCursor(100, 40); display.print("M2");
  
  // Abajo: Motores Traseros (M3, M1)
  display.fillRect(0, 50, w3, 4, WHITE);  display.setCursor(35, 50); display.print("M3");
  display.fillRect(64, 50, w1, 4, WHITE); display.setCursor(100, 50); display.print("M1");

  display.display();
}