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

// ==========================================
//      PID Preset สำหรับใบพัด 7035 + มอเตอร์ 1300KV
// ==========================================

// --- PID Parameters ---
float P_yaw, I_yaw, D_yaw;
float P_angle, P_rate, I_gain;

// --- Tuning Parameters ---
float DEAD_BAND_ROLL = 1.0f;
float DEAD_BAND_PITCH = 1.0f;
float DEAD_BAND_YAW = 1.0f;  // ✅ Deadband ของ errY
float I_ACCUM_YAW = 0.002f;  // ✅ ค่าการสะสม I-term

void setupPIDPresets() {
  // --- Yaw PID ---
  P_yaw = 0.58f; // ค่าเดิม 0.58f;
  I_yaw = 0.18f;  // ค่าเดิม 0.18f;
  D_yaw = 0.015f;

  // --- Roll & Pitch PID ---
  P_angle = 0.36f;  // ค่าเดิม 0.46f;
  P_rate = 0.050f;  // ค่าเดิม 0.035f;
  I_gain = 0.015f;  // ค่าเดิม I_gain = 0.025f;
}

const float SMOOTH_FACTOR = 0.85f;  // เพิ่มค่า เพื่อให้มอเตอร์เดินเรียบขึ้น (ลดเสียงคึกคึก) สูงสุด 1.00f
const float IDLE_POWER = 28.0f;     // ลดลงเล็กน้อยให้ลงจอดง่ายขึ้น
const float STICK_EXPO = 0.45f;     // เพิ่มความนุ่มนวลช่วงกลางจอย
const float angleR_off = -0.0016f;    //  ให้เริ่มที่ 0.00f การปรับ offset เล็กน้อยเพื่อแก้ bias ของเซนเซอร์หรือการติดตั้ง เช่น ใส่ค่าติดลบ(-) outR จะได้ค่า (-) เพื่อเร่ง Motor คู่ขวา แก้ไหลไปขวา
const float angleP_off = -0.003525f;    // ให้เริ่มที่ 0.00f การปรับ offset เล็กน้อยเพื่อแก้ bias ของเซนเซอร์หรือการติดตั้ง เช่น ใส่ค่าติดลบ(-) outP จะได้ค่า (+) เพื่อเร่ง Motor คู่หลัง แก้ไหลไปหลัง
const float yaw_off = -50.0f;    // ปรับ offset เล็กน้อยเพื่อแก้ bias ของเซนเซอร์หรือการติดตั้ง ใส่ค่า(-) outY จะได้ค่า (+) เพื่อเร่ง Motor คู่หมุนทวนเข็ม แก้ไหลทวนเข็มนาฬิกา
#define LOOP_TIME_MS 1
#define BAT_PIN 34  // ขาอ่านแรงดันแบตเตอรี่

// ==========================================
//      2. PIN DECLARATION
// ==========================================
#define M1_PIN 12
#define M2_PIN 14
#define M3_PIN 18 
#define M4_PIN 19
const int MOTOR_PINS[] = { M1_PIN, M2_PIN, M3_PIN, M4_PIN };
#define LED_GREEN 2
#define LED_RED 4

Adafruit_MPU6050 mpu;
Preferences prefs;

volatile long ch_raw[4] = { 1500, 1500, 1000, 1500 };
volatile unsigned long ch_start[4], last_signal_time = 0;
float accX_off = 0, accY_off = 0, gyroX_off = 0, gyroY_off = 0, gyroZ_off = 0;
float roll_mid = 1500, pitch_mid = 1500, yaw_mid = 1500, roll_i = 0, pitch_i = 0, yaw_i = 0, angleR = 0, angleP = 0, m_last[4] = { 0, 0, 0, 0 };

float outP;
float outR;
float outY;
float targetP;
float targetR;
float targetY;
static float last_outR_final;
static float last_outP_final;

// --- Failsafe Settings ---
bool is_failsafe = false;
float failsafe_thr = 0;  // ตัวแปรเก็บค่า throttle ขณะเกิด Failsafe
unsigned long failsafe_timer = 0;

// ฟังก์ชันกรองค่า RC Input
int readChannelSafe(int chValue, int channel) {
  chValue = constrain(chValue, 1000, 2000);
  static int lastValue[4] = { 1500, 1500, 1500, 1500 };

  // ✅ เพิ่มจุดนี้: จำกัดการเปลี่ยนแปลงไม่เกิน 50 หน่วยต่อรอบ
  int diff = chValue - lastValue[channel];
  if (diff > 50) chValue = lastValue[channel] + 50;
  if (diff < -50) chValue = lastValue[channel] - 50;

  chValue = (int)(0.9f * lastValue[channel] + 0.1f * chValue);
  lastValue[channel] = chValue;
  return chValue;
}

// --- Functions ---
float applyExpo(float input, float expo) {
  return input * (1.0f - expo) + (input * input * input * expo);
}

// --- Notch Filter Variables ---
float nx1 = 0, nx2 = 0, ny1 = 0, ny2 = 0; // สำหรับแกน X (Roll)
float npx1 = 0, npx2 = 0, npy1 = 0, npy2 = 0; // สำหรับแกน Y (Pitch)

// ฟังก์ชัน Notch Filter (200Hz Center, 1000Hz Sample Rate)
float notchFilter(float input, float &x1, float &x2, float &y1, float &y2) {
  // Coefficients calculated for 200Hz notch @ 1kHz rate
  const float b0 = 0.8192f;
  const float b1 = -0.5063f;
  const float b2 = 0.8192f;
  const float a1 = -0.5063f;
  const float a2 = 0.6384f;

  float output = (b0 * input) + (b1 * x1) + (b2 * x2) - (a1 * y1) - (a2 * y2);

  // Shift values
  x2 = x1;
  x1 = input;
  y2 = y1;
  y1 = output;

  return output;
}


float medianFilterR(float newR) {
  static float r1 = 0, r2 = 0;
  float resultR;
  // หาค่ากลางจาก 3 ค่าล่าสุด (newR, r1, r2)
  if ((r1 <= newR && newR <= r2) || (r2 <= newR && newR <= r2)) resultR = newR;
  else if ((newR <= r1 && r1 <= r2) || (r2 <= r1 && r1 <= newR)) resultR = r1;
  else resultR = r2;

  r2 = r1;
  r1 = newR;
  return resultR;
}

float medianFilterP(float newP) {
  static float p1 = 0, p2 = 0;
  float resultP;
  // หาค่ากลางจาก 3 ค่าล่าสุด (newP, p1, p2)
  if ((p1 <= newP && newP <= p2) || (p2 <= newP && newP <= p1)) resultP = newP;
  else if ((newP <= p1 && p1 <= p2) || (p2 <= p1 && p1 <= newP)) resultP = p1;
  else resultP = p2;

  p2 = p1;
  p1 = newP;
  return resultP;
}

// --- Battery Settings ---
#define BAT_PIN 34
float volt_divider_ratio = 4.92f;  // ปรับตามจริง ( (R1+R2)/R2 )
float filtered_volts = 12.0f;
float low_bat_threshold = 10.5f;  // สำหรับ 3S (3.5V ต่อเซลล์)

// ฟังก์ชันอ่านแรงดันแบบกรองสัญญาณ (Low-pass Filter)
float readBattery() {
  int raw_adc = analogRead(BAT_PIN);
  // ESP32 ADC มี 12-bit (0-4095) แรงดันอ้างอิงประมาณ 3.3V
  float sample = (raw_adc / 4095.0f) * 3.3f * volt_divider_ratio;
  // กรองสัญญาณให้นิ่ง (ใช้ 99% ค่าเก่า + 1% ค่าใหม่)
  filtered_volts = (filtered_volts * 0.99f) + (sample * 0.01f);
  return filtered_volts;
}

// --- ISR Radio ---
void IRAM_ATTR isr1() {
  if (digitalRead(32)) ch_start[0] = micros();
  else {
    ch_raw[0] = constrain(micros() - ch_start[0], 1000, 2000);
    last_signal_time = millis();
  }
}

void IRAM_ATTR isr2() {
  if (digitalRead(33)) ch_start[1] = micros();
  else {
    ch_raw[1] = constrain(micros() - ch_start[1], 1000, 2000);
    last_signal_time = millis();
  }
}

void IRAM_ATTR isr3() {
  if (digitalRead(25)) ch_start[2] = micros();
  else {
    ch_raw[2] = constrain(micros() - ch_start[2], 1000, 2000);
    last_signal_time = millis();
  }
}

void IRAM_ATTR isr4() {
  if (digitalRead(26)) ch_start[3] = micros();
  else {
    ch_raw[3] = constrain(micros() - ch_start[3], 1000, 2000);
    last_signal_time = millis();
  }
}

void writeMotors(float m1, float m2, float m3, float m4) {
  float targets[4] = { m1, m2, m3, m4 };
  float invS = 1.0f - SMOOTH_FACTOR;
  for (int i = 0; i < 4; i++) {
    targets[i] = (m_last[i] * SMOOTH_FACTOR) + (targets[i] * invS);
    m_last[i] = targets[i];
    float power = constrain(targets[i], 0, 100);
    uint32_t duty = 8192 + (uint32_t)(power * 81.92f);
    ledcWrite(MOTOR_PINS[i], duty);
  }
}

// ---  FlightControl ---
float thr;
void FlightControlTask(void *pv) {
  TickType_t xLastWakeTime = xTaskGetTickCount();
  for (;;) {
    vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(LOOP_TIME_MS));
    /*

    // --- อ่านแรงดันแบตเตอรี่ ---
    int raw_adc = readBattery();
    // --- Low Battery Warning ---
    if(raw_adc < low_bat_threshold && !is_failsafe){
      digitalWrite(LED_GREEN, LOW);
      digitalWrite(LED_RED, (millis() / 200) % 2);  // ไฟแดงติดกระพริบเตือน Low Battery
    }
    */

    // --- Throttle ---
    thr = map(ch_raw[2], 1000, 2000, 0, 100);
    if (thr >= IDLE_POWER) {
      digitalWrite(LED_GREEN, (millis() / 200) % 2);  // ไฟเขียวกระพริบขณะบิน
    }

    // --- ตรวจสอบสัญญาณรีโมท (Failsafe Check) ---
    if (millis() - last_signal_time > 500) {
      if (!is_failsafe) {
        is_failsafe = true;
        digitalWrite(LED_GREEN, LOW);
        failsafe_thr =  (thr < 5)? 0 : thr;  // ถ้าจอดอยู่ให้เป็น 0 แต่่ถ้าบินอยู่ให้จำค่าไว้
        failsafe_timer = millis();
      }

      // ค่อยๆ ลดคันเร่งลง วินาทีละ 5%
      if (millis() - failsafe_timer > 100) {
        if (failsafe_thr > 0) failsafe_thr -= 0.25f;  // ลดลงทีละนิด
        failsafe_timer = millis();
      }

      if (failsafe_thr < 5) {  // ถ้าคันเร่งต่ำมากแล้ว ให้ดับเครื่อง
        writeMotors(0, 0, 0, 0);
        roll_i = pitch_i = yaw_i = 0;
        outR = outP = outY = 0;
        thr = failsafe_thr = 0;
        digitalWrite(LED_GREEN, LOW);
        digitalWrite(LED_RED, (millis() / 200) % 2);  // ไฟแดงติดกระพริบ เตือนสัญญาณหลุด
        continue;
      }

      // ใช้ค่า failsafe_thr แทนค่าจากรีโมท
      thr = failsafe_thr;

      // รักษาระดับ (Roll/Pitch เป็น 0) เพื่อให้ลงจอดตรงๆ
      targetR = 0;
      targetP = 0;
      targetY = 0;
    } else {
      is_failsafe = false;  // สัญญาณกลับมาแล้ว คืนการควบคุมให้ผู้บังคับการบิน
      digitalWrite(LED_RED, LOW);
    }

    // --- Sensor Data ---
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);
    float gx = (g.gyro.x - gyroX_off) * (180.0f / M_PI);
    float gy = (g.gyro.y - gyroY_off) * (180.0f / M_PI);

    //float gz = (g.gyro.z - gyroZ_off) * (180.0f / M_PI); // เดิม

    // ✅ Reverse Yaw
    float gz = -(g.gyro.z - gyroZ_off) * (180.0f / M_PI);

    float rA = atan2(a.acceleration.y - accY_off, a.acceleration.z) * (180.0f / M_PI);
    float pA = atan2(-(a.acceleration.x - accX_off),
                     sqrt(pow(a.acceleration.y - accY_off, 2) + pow(a.acceleration.z, 2)))
               * (180.0f / M_PI);
    angleR = (0.999f * (angleR + gx * 0.001f) + 0.001f * rA) - angleR_off;
    angleP = (0.999f * (angleP + gy * 0.001f) + 0.001f * pA) + angleP_off;

    // ✅ Reset PID เมื่อ throttle ต่ำกว่า 8%
    if (thr < 5) {
      writeMotors(0, 0, 0, 0);
      roll_i = pitch_i = yaw_i = 0;
      outR = outP = outY = 0;
      targetR = 0;
      targetP = 0;
      targetY = 0;  // ✅ reset target ด้วย
      digitalWrite(LED_GREEN, HIGH);
      digitalWrite(LED_RED, LOW);
      continue;  // ข้ามไป loop ถัดไป
    }

    // --- อ่านค่า RC ผ่าน RC Filter ---
    int roll = readChannelSafe(ch_raw[0], 0);
    int pitch = readChannelSafe(ch_raw[1], 1);
    int yaw = readChannelSafe(ch_raw[3], 3);

    float rawR = (roll - roll_mid) / 500.0f;
    float rawP = (pitch - pitch_mid) / 500.0f;
    float rawY = (yaw - yaw_mid) / 500.0f;

    if (fabs(rawR) < 0.05f) rawR = 0;
    if (fabs(rawP) < 0.05f) rawP = 0;
    if (fabs(rawY) < 0.1f) rawY = 0;

    targetR = (applyExpo(constrain(rawR, -1, 1), STICK_EXPO) * 20.0f);
    targetP = (applyExpo(constrain(rawP, -1, 1), STICK_EXPO) * 20.0f);
    targetY = applyExpo(constrain(rawY, -1, 1), STICK_EXPO) * 35.0f;

    // --- TPA Logic ---
    float tpa = 1.0f;
    if (thr > 50) {
      tpa = 1.0f - (constrain(thr - 50, 0, 50) * 0.01f);
    }

    // --- PID Roll ---
    static float outR_smooth = 0;
    last_outR_final = 0;
    float errR = targetR - angleR;
    if (fabs(errR) < DEAD_BAND_ROLL) errR = 0;
    if (thr < 5) roll_i = 0;  // ล้างค่า I-term ตอนจอด
    else roll_i = constrain(roll_i + errR * 0.001f, -2.0f, 2.0f);
    outR = ((P_angle * errR) + (I_gain * roll_i) - (P_rate * gx)) * tpa;

    float medR = medianFilterR(outR);
    outR_smooth = (outR_smooth * 0.85f) + (medR * 0.15f);
    outR = outR_smooth;
    // จำกัดให้ outR ห้ามกระขากเกิน 0.8 หน่วยต่อ 1ms
    //outR = constrain(outR_smooth, last_outR_final - 0.8f, last_outR_final + 0.8f);
    //last_outR_final = outR;

    // --- PID Pitch ---
    static float outP_smooth = 0;
    last_outP_final = 0;
    float errP = targetP - angleP;
    if (fabs(errP) < DEAD_BAND_PITCH) errP = 0;
    if (thr < 5) pitch_i = 0;  // ล้างค่า I-term ตอนจอด
    else pitch_i = constrain(pitch_i + errP * 0.001f, -2.0f, 2.0f);
    outP = ((P_angle * errP) + (I_gain * pitch_i) - (P_rate * gy)) * tpa;

    float medP = medianFilterR(outP);
    outP_smooth = (outP_smooth * 0.85f) + (medP * 0.15f);
    outP = outP_smooth;
    // จำกัดให้ outP ห้ามกระขากเกิน 0.8 หน่วยต่อ 1ms
    //outP = constrain(outP_smooth, last_outP_final - 0.8f, last_outP_final + 0.8f);
    //last_outP_final = outP;
    
    // --- PID Yaw ---
    float errY = targetY - gz;
    if (fabs(errY) < DEAD_BAND_YAW) errY = 0;

    if (fabs(rawY) < 0.1f) yaw_i = 0;
    else yaw_i = constrain(yaw_i + errY * I_ACCUM_YAW, -6.5f, 6.5f);

    outY = ((P_yaw * errY) + (I_yaw * yaw_i) - (D_yaw * gz)) * tpa;
    outY = constrain(outY, -6.5f, 6.5f);

    float base = thr + IDLE_POWER;
    
    // ส่งค่าไปที่มอเตอร์แบบเดิม
    // เพิ่มระบบ Anti-Windup ช่วยโดรน เวลาลมกระโชกแรง
    // --- motor Mixing with New Smooth Factor ---
    float m1_target = base - outR + outP - outY;  // M1 หลัง-ขวา (CW)
    float m2_target = base - outR - outP + outY;  // M2 หน้า-ขวา (CCW)
    float m3_target = base + outR + outP + outY;  // M3 หลัง-ซ้าย (CCW)
    float m4_target = base + outR - outP - outY;  // M4 หน้า-ซ้าย (CW)

    float targets[] = { m1_target, m2_target, m3_target, m4_target };
    float invS = 1.0f - SMOOTH_FACTOR;

    for (int i = 0; i < 4; i++) {
      m_last[i] = (m_last[i] * SMOOTH_FACTOR) + (targets[i] * invS);
      float power = constrain(m_last[i], 0, 100);
      uint32_t duty = 8192 + (uint32_t)(power * 81.92f);
      ledcWrite(MOTOR_PINS[i], duty);
    }
  }
}

void setup() {
  pinMode(LED_GREEN, OUTPUT);
  pinMode(LED_RED, OUTPUT);
  Serial.begin(115200);
  Wire.begin(21, 22);
  Wire.setClock(400000);
  if (!mpu.begin())
    while (1)
      ;
  // [จุดสำคัญ] ปรับ Filter Bandwidth เพื่อลดเสียงคึกคึก
  mpu.setFilterBandwidth(MPU6050_BAND_44_HZ);  // เดิม 21 Hz

  for (int i = 0; i < 4; i++) {
    ledcAttachChannel(MOTOR_PINS[i], 1000, 16, i);
    ledcWrite(MOTOR_PINS[i], 0);  // ส่งสัญญาณ 0 ให้ ESC
  }

  pinMode(32, INPUT_PULLDOWN);
  attachInterrupt(32, isr1, CHANGE);
  pinMode(33, INPUT_PULLDOWN);
  attachInterrupt(33, isr2, CHANGE);
  pinMode(25, INPUT_PULLDOWN);
  attachInterrupt(25, isr3, CHANGE);
  pinMode(26, INPUT_PULLDOWN);
  attachInterrupt(26, isr4, CHANGE);

  // Sequence การเริ่มระบบ (เหมือนเดิม)
  while (last_signal_time == 0) {
    digitalWrite(LED_RED, (millis() / 50) % 2);
    delay(10);
  }
  while (ch_raw[3] < 1800) {
    digitalWrite(LED_RED, (millis() / 100) % 2);
    digitalWrite(LED_GREEN, !((millis() / 100) % 2));
    delay(10);
  }

  // โหมด Calibrate/Ready (เหมือนเดิม)
  bool readyToFly = false;
  unsigned long calTimer = 0;
  bool throttleUpTriggered = false;
  while (!readyToFly) {
    if (ch_raw[3] > 1800 && ch_raw[1] < 1200 && ch_raw[0] < 1200) {
      if (calTimer == 0) calTimer = millis();
      digitalWrite(LED_GREEN, (millis() / 50) % 2);
      if (millis() - calTimer > 3000) {
        digitalWrite(LED_RED, HIGH);

        float ax_s = 0, ay_s = 0;
        float gx_s = 0, gy_s = 0, gz_s = 0;
        for (int i = 0; i < 400; i++) {
          sensors_event_t a, g, temp;
          mpu.getEvent(&a, &g, &temp);
          ax_s += a.acceleration.x;
          ay_s += a.acceleration.y;
          gx_s += g.gyro.x;
          gy_s += g.gyro.y;
          gz_s += g.gyro.z;
          delay(5);
        }
        accX_off = ax_s / 400.0f;
        accY_off = ay_s / 400.0f;
        gyroX_off = gx_s / 400.0f;
        gyroY_off = gy_s / 400.0f;
        gyroZ_off = gz_s / 400.0f;
        prefs.begin("drone-lv", false);
        prefs.putFloat("ax", accX_off);
        prefs.putFloat("ay", accY_off);
        prefs.putFloat("gx_off", gyroX_off);
        prefs.putFloat("gy_off", gyroY_off);
        prefs.putFloat("gz_off", gyroZ_off);
        prefs.end();

        digitalWrite(LED_RED, LOW);
        delay(10);
        writeMotors(0, 0, 0, 0);
        calTimer = 0;
        digitalWrite(LED_GREEN, (millis() / 500) % 2);
        //readyToFly = true;
      }
    } else {
      calTimer = 0;
      digitalWrite(LED_GREEN, (millis() / 500) % 2);
      digitalWrite(LED_RED, LOW);
      if (ch_raw[2] > 1350) throttleUpTriggered = true;
      if (throttleUpTriggered && ch_raw[2] < 1050) readyToFly = true;
    }
    delay(20);
  }

  // --- RC Mid-point Calibration ---
  long sumRoll = 0, sumPitch = 0, sumYaw = 0;
  for (int i = 0; i < 100; i++) {  // ✅ เปลี่ยนจาก 50 → 100 sample
    sumRoll += ch_raw[0];
    sumPitch += ch_raw[1];
    sumYaw += ch_raw[3];
    delay(20);  // รอให้รีซีฟเวอร์ส่งค่า (20ms x 100 ≈ 2 วินาที)
  }

  // เก็บค่า mid-point ที่แม่นยำขึ้น
  roll_mid = sumRoll / 100;
  pitch_mid = sumPitch / 100;
  yaw_mid = sumYaw / 100;

  // ✅ โหลด offset ที่บันทึกไว้กลับมาใช้งานจริง
  prefs.begin("drone-lv", false);
  accX_off = prefs.getFloat("ax", 0.0f);
  accY_off = prefs.getFloat("ay", 0.0f);
  gyroX_off = prefs.getFloat("gx_off", 0.0f);
  gyroY_off = prefs.getFloat("gy_off", 0.0f);
  gyroZ_off = prefs.getFloat("gz_off", 0.0f);
  prefs.end();
  // ✅ รวม offset เข้ากับ mid-point
  roll_mid += angleR_off;
  pitch_mid += angleP_off;
  yaw_mid += yaw_off;
  setupPIDPresets();
  // บรรทัดนี้ต้องอยู่หลังการ Calibrate เสมอ
  xTaskCreatePinnedToCore(FlightControlTask, "Flight", 8192, NULL, 24, NULL, 0);
}

void loop() {

  //Serial.print("angleR: ");
 // Serial.print(angleR);
  //Serial.print("  angleP: ");
  //Serial.println(angleP);

  //Serial.print("targetR: ");
  //Serial.print(targetR);
  //Serial.print("  targetP: ");
  //Serial.print(targetP);
  //Serial.print("  yaw_mid: ");
  //Serial.print(yaw_mid);
 // Serial.print("  CH4 Yaw: ");
  //Serial.print(ch_raw[3]);
  //Serial.print("  targetY: ");
  //Serial.println(targetY);
/*
  Serial.print("outR:");
  Serial.print(outR);
  Serial.print(",");
  Serial.print("outP:");
  Serial.println(outP);
*/
  
  //Serial.print("  outY: ");
  //Serial.println(outY);


/*
  Serial.print("CH1 Roll: ");
  Serial.println(ch_raw[0]);
  Serial.print("CH2 Pitch: ");
  Serial.println(ch_raw[1]);
  Serial.print("CH3 Throttle: ");
  Serial.println(ch_raw[2]);
  Serial.print("CH4 Yaw: ");
  Serial.println(ch_raw[3]);  */

  /*
  Serial.print("Bat: "); 
  Serial.print(filtered_volts); 
  Serial.println(" V"); */

  /*
  Serial.print("failsafe_thr=");
  Serial.println(failsafe_thr);
  Serial.print("thr=");
  Serial.println(thr); */

  delay(200);
}