/*
  MuTech Robotics - DR1 ESP32 Mini Drone (custom PCB frame, 65 mm, 4 x 720 coreless motors)
  CONCEPT / LEARNING CODE - not tested on hardware. Test with PROPS OFF first, then tune the PID values.

  Board (all on one custom PCB):
    ESP32 module, 3.3 V low-dropout regulator (from the 1S LiPo), 6-axis motion sensor (I2C, address 0x68),
    4 x logic-level N-MOSFET low-side motor drivers (gate: 100 ohm series + 10k pull-down to GND),
    a Schottky flyback diode across every motor, battery divider 100k/100k to GPIO34.
  Pins
    Motors (MOSFET gates): FL GPIO25 (CW) | FR GPIO26 (CCW) | RL GPIO27 (CCW) | RR GPIO14 (CW)
    Motion sensor: SDA GPIO21, SCL GPIO22 | Battery sense: GPIO34 | Status LED: GPIO2
  Motors: CW = red/blue wires, CCW = black/white wires (common convention - check yours by spinning slowly).
  Props: diagonal pairs spin the same way. Fit the matching CW / CCW props.

  Phone control: the drone makes its own Wi-Fi hotspot "MuTech-Drone" (password below).
  Join it, open http://192.168.4.1 and use the two on-screen joysticks. Tap ARM to start, DISARM to stop.
  Library: "WebSockets" by Markus Sattler (Library Manager). Board: ESP32 Arduino core 3.x.

  Safety: motors stop if the phone signal is lost for 0.5 s, and it lands when the battery is low.
  Keep fingers away from the props. Fly indoors in an open room. Check your local drone rules.
*/
#include <WiFi.h>
#include <WebServer.h>
#include <WebSocketsServer.h>
#include <Wire.h>

const char* AP_SSID = "MuTech-Drone";
const char* AP_PASS = "fly12345";            // change it (8+ characters)
const int MOTOR[4] = {25, 26, 27, 14};       // FL, FR, RL, RR
const int LED = 2, VBAT = 34;
const int PWM_FREQ = 20000, PWM_BITS = 10, PWM_MAX = 1023;

// PID gains (start small, tune with props off then low throttle)
float KP_ANG = 4.0;                         // angle -> rate
float KP_RATE = 0.9, KI_RATE = 0.6, KD_RATE = 0.02;
float KP_YAW = 1.6, KI_YAW = 0.4;
const float MAX_ANGLE = 25.0, MAX_YAW_RATE = 120.0;

WebServer http(80);
WebSocketsServer ws(81);
volatile float cmdT = 0, cmdY = 0, cmdP = 0, cmdR = 0;   // throttle 0..1, others -1..1
volatile bool armed = false; volatile unsigned long lastCmd = 0;

// ---------------- motion sensor
float gx, gy, gz, ax, ay, az, gbx = 0, gby = 0, gbz = 0;
float roll = 0, pitch = 0;                   // degrees
void imuWrite(uint8_t r, uint8_t v) { Wire.beginTransmission(0x68); Wire.write(r); Wire.write(v); Wire.endTransmission(); }
void imuRead() {
  Wire.beginTransmission(0x68); Wire.write(0x3B); Wire.endTransmission(false);
  Wire.requestFrom(0x68, 14);
  int16_t v[7]; for (int i = 0; i < 7; i++) v[i] = (Wire.read() << 8) | Wire.read();
  ax = v[0] / 8192.0; ay = v[1] / 8192.0; az = v[2] / 8192.0;               // +-4 g
  gx = v[4] / 65.5 - gbx; gy = v[5] / 65.5 - gby; gz = v[6] / 65.5 - gbz;   // +-500 deg/s
}
void imuInit() {
  imuWrite(0x6B, 0x00); delay(50);           // wake
  imuWrite(0x1A, 0x03);                       // low-pass filter ~44 Hz
  imuWrite(0x1B, 0x08);                       // gyro +-500 deg/s
  imuWrite(0x1C, 0x08);                       // accel +-4 g
  float sx = 0, sy = 0, sz = 0;
  for (int i = 0; i < 500; i++) { imuRead(); sx += gx; sy += gy; sz += gz; delay(2); }   // keep it still!
  gbx = sx / 500; gby = sy / 500; gbz = sz / 500;
}

// ---------------- motors
void motorsInit() { for (int i = 0; i < 4; i++) { ledcAttach(MOTOR[i], PWM_FREQ, PWM_BITS); ledcWrite(MOTOR[i], 0); } }
void motorsWrite(float m[4]) { for (int i = 0; i < 4; i++) ledcWrite(MOTOR[i], (int)(constrain(m[i], 0, 1) * PWM_MAX)); }
void motorsOff() { float z[4] = {0, 0, 0, 0}; motorsWrite(z); }

float battery() { return analogReadMilliVolts(VBAT) * 2 / 1000.0; }       // 100k/100k divider

// ---------------- phone page (two joysticks)
const char PAGE[] PROGMEM = R"HTML(
<!doctype html><html><head><meta name=viewport content="width=device-width,initial-scale=1,user-scalable=no">
<style>body{margin:0;background:#18202c;color:#fff;font-family:sans-serif;touch-action:none;user-select:none}
.j{position:absolute;bottom:40px;width:42vw;height:42vw;max-width:260px;max-height:260px;border-radius:50%;background:#2a3446}
#L{left:5vw}#R{right:5vw}.k{position:absolute;width:30%;height:30%;left:35%;top:35%;border-radius:50%;background:#ffa83c}
#top{padding:14px;text-align:center}button{font-size:20px;padding:10px 26px;margin:6px;border:0;border-radius:20px}</style></head>
<body><div id=top>MuTech Drone <span id=b></span><br><button onclick="send('ARM')" style="background:#3c3">ARM</button><button onclick="send('DISARM')" style="background:#e44;color:#fff">DISARM</button></div>
<div class=j id=L><div class=k></div></div><div class=j id=R><div class=k></div></div>
<script>
const s=new WebSocket('ws://'+location.hostname+':81/');s.onmessage=e=>document.getElementById('b').textContent=e.data;
const st={L:[0,1],R:[0,0]};function send(m){if(s.readyState==1)s.send(m)}
for(const id of ['L','R']){const el=document.getElementById(id),k=el.firstChild;
 const mv=e=>{const r=el.getBoundingClientRect();for(const t of e.touches){if(t.target.closest('#'+id)){let x=(t.clientX-r.left)/r.width*2-1,y=(t.clientY-r.top)/r.height*2-1;
  x=Math.max(-1,Math.min(1,x));y=Math.max(-1,Math.min(1,y));st[id]=[x,y];k.style.left=(35+x*35)+'%';k.style.top=(35+y*35)+'%';}}e.preventDefault()};
 el.addEventListener('touchstart',mv);el.addEventListener('touchmove',mv);
 el.addEventListener('touchend',()=>{if(id=='R'){st.R=[0,0];k.style.left=k.style.top='35%'}else{st.L[0]=0;k.style.left='35%'}});}
setInterval(()=>send('C,'+((1-st.L[1])/2).toFixed(3)+','+st.L[0].toFixed(3)+','+(-st.R[1]).toFixed(3)+','+st.R[0].toFixed(3)),50);
</script></body></html>)HTML";

void onWs(uint8_t num, WStype_t type, uint8_t* payload, size_t len) {
  if (type != WStype_TEXT) return;
  String m = String((char*)payload).substring(0, len);
  if (m == "ARM") { if (cmdT < 0.05) armed = true; }
  else if (m == "DISARM") armed = false;
  else if (m.startsWith("C,")) {
    float v[4]; int p = 2;
    for (int i = 0; i < 4; i++) { int c = m.indexOf(',', p); v[i] = m.substring(p, c < 0 ? m.length() : c).toFloat(); p = c + 1; }
    cmdT = constrain(v[0], 0, 1); cmdY = constrain(v[1], -1, 1); cmdP = constrain(v[2], -1, 1); cmdR = constrain(v[3], -1, 1);
    lastCmd = millis();
  }
}

void setup() {
  pinMode(LED, OUTPUT);
  motorsInit();
  Wire.begin(21, 22, 400000);
  imuInit();
  WiFi.softAP(AP_SSID, AP_PASS);
  http.on("/", [] { http.send_P(200, "text/html", PAGE); }); http.begin();
  ws.begin(); ws.onEvent(onWs);
}

float iR = 0, iP = 0, iY = 0, lastER = 0, lastEP = 0;
unsigned long lastLoop = 0, lastTel = 0;
void loop() {
  http.handleClient(); ws.loop();
  unsigned long now = micros();
  if (now - lastLoop < 2000) return;              // 500 Hz control loop
  float dt = (now - lastLoop) / 1e6; lastLoop = now;
  imuRead();
  // complementary filter (gyro + accelerometer)
  float accR = atan2(ay, az) * 57.3, accP = atan2(-ax, sqrt(ay * ay + az * az)) * 57.3;
  roll = 0.98 * (roll + gx * dt) + 0.02 * accR;
  pitch = 0.98 * (pitch + gy * dt) + 0.02 * accP;

  bool linkOk = millis() - lastCmd < 500;
  float vb = battery();
  if (!linkOk || !armed) { armed = armed && linkOk; motorsOff(); iR = iP = iY = 0; digitalWrite(LED, (millis() / 500) % 2); return; }
  digitalWrite(LED, HIGH);
  float thr = cmdT;
  if (vb < 3.3) thr = min(thr, 0.35f);            // low battery: gentle descent

  // angle mode: stick -> target angle -> target rate -> PID on rate
  float rateR = KP_ANG * (cmdR * MAX_ANGLE - roll), rateP = KP_ANG * (cmdP * MAX_ANGLE - pitch);
  float eR = rateR - gx, eP = rateP - gy, eY = cmdY * MAX_YAW_RATE - gz;
  if (thr > 0.15) { iR += eR * dt; iP += eP * dt; iY += eY * dt; }
  iR = constrain(iR, -50, 50); iP = constrain(iP, -50, 50); iY = constrain(iY, -50, 50);
  float oR = (KP_RATE * eR + KI_RATE * iR + KD_RATE * (eR - lastER) / dt) / 1000.0;
  float oP = (KP_RATE * eP + KI_RATE * iP + KD_RATE * (eP - lastEP) / dt) / 1000.0;
  float oY = (KP_YAW * eY + KI_YAW * iY) / 1000.0;
  lastER = eR; lastEP = eP;

  // quad X mixer: FL CW, FR CCW, RL CCW, RR CW
  // roll right -> left motors up; nose down (forward) -> rear motors up; yaw right -> CCW motors up
  float m[4];
  m[0] = thr + oR - oP - oY;   // FL
  m[1] = thr - oR - oP + oY;   // FR
  m[2] = thr + oR + oP + oY;   // RL
  m[3] = thr - oR + oP - oY;   // RR
  if (thr < 0.05) for (int i = 0; i < 4; i++) m[i] = 0;
  motorsWrite(m);
  if (millis() - lastTel > 1000) { lastTel = millis(); ws.broadcastTXT(String("battery ") + String(vb, 2) + " V"); }
}