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

Adafruit_MPU6050 mpu;

// -------------------- CONFIGURAÇÃO DOS LEDS --------------------
struct LedGroup { uint8_t lowPin, midPin, highPin; };
enum Corner { FL=0, FR=1, BL=2, BRK=3 };  // BRK para evitar conflito com macro BR

LedGroup groups[4] = {
  /* FL  */ {16, 17,  5},
  /* FR  */ {18, 19, 23},
  /* BL  */ {25, 26, 27},
  /* BRK */ {32, 33,  4}
};

// ---------------------- PARÂMETROS DE NÍVEL --------------------
float pitch0 = 0.0f, roll0 = 0.0f;
const float THRESH_DEG = 1.5f;
const float HYST_DEG   = 0.5f;
const float LPF_ALPHA  = 0.12f;
const float TRANSITION_BAND = 0.5f;

float pitchFilt = 0.0f, rollFilt = 0.0f;

// ---------------------- ESTADO / CALIBRAÇÃO --------------------
enum State { ST_LOW=-1, ST_MID=0, ST_HIGH=1 };
State lastState[4] = {ST_MID, ST_MID, ST_MID, ST_MID};

enum RunMode { CALIBRATING, RUNNING };
RunMode mode = CALIBRATING;

const uint32_t CAL_TIME_MS = 2000; // 2 s
uint32_t calStartMs = 0;
double calSumPitch = 0.0, calSumRoll = 0.0;
uint32_t calSamples = 0;

// ---------------------- FUNÇÕES AUXILIARES ---------------------
void setGroup(Corner c, State s, bool alsoMid) {
  digitalWrite(groups[c].lowPin,  (s == ST_LOW)  ? HIGH : LOW);
  digitalWrite(groups[c].midPin,  (s == ST_MID || alsoMid) ? HIGH : LOW);
  digitalWrite(groups[c].highPin, (s == ST_HIGH) ? HIGH : LOW);
}

void setAllLEDs(bool state) {
  for (auto &g : groups) {
    digitalWrite(g.lowPin,  state ? HIGH : LOW);
    digitalWrite(g.midPin,  state ? HIGH : LOW);
    digitalWrite(g.highPin, state ? HIGH : LOW);
  }
}

void blinkAllLEDs(int times, int delayMs) {
  for (int i = 0; i < times; i++) {
    setAllLEDs(true);
    delay(delayMs);
    setAllLEDs(false);
    delay(delayMs);
  }
}

State decideWithHysteresis(Corner c, float value) {
  float upTh   =  THRESH_DEG;
  float downTh = -THRESH_DEG;
  if (lastState[c] == ST_HIGH) downTh = -(THRESH_DEG - HYST_DEG);
  if (lastState[c] == ST_LOW ) upTh   =  (THRESH_DEG - HYST_DEG);

  State s = ST_MID;
  if (value > upTh)        s = ST_HIGH;
  else if (value < downTh) s = ST_LOW;
  lastState[c] = s;
  return s;
}

void updateCornerLEDs(float pitchDeg, float rollDeg) {
  struct G { int kPitch; int kRoll; } K[4] = {
    /* FL  */ {+1, -1},
    /* FR  */ {+1, +1},
    /* BL  */ {-1, -1},
    /* BRK */ {-1, +1},
  };

  for (int i=0; i<4; i++) {
    float s = K[i].kPitch * pitchDeg + K[i].kRoll * rollDeg;
    State st = decideWithHysteresis((Corner)i, s);

    bool alsoMid = false;
    if (fabs(s) > (THRESH_DEG - TRANSITION_BAND) && fabs(s) <= THRESH_DEG) {
      alsoMid = true;
    }

    setGroup((Corner)i, st, alsoMid);
  }
}

void setAllMidOn() {
  for (int i=0; i<4; i++) {
    setGroup((Corner)i, ST_MID, false);
  }
}

// --------------------------- SETUP -----------------------------
void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);

  for (auto &g : groups) {
    pinMode(g.lowPin,  OUTPUT);
    pinMode(g.midPin,  OUTPUT);
    pinMode(g.highPin, OUTPUT);
  }

  if (!mpu.begin()) {
    Serial.println("MPU6050 não encontrado. Verifique conexões/instância no Wokwi.");
    while (1) delay(10);
  }

  mpu.setAccelerometerRange(MPU6050_RANGE_4_G);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  mode = CALIBRATING;
  calStartMs = millis();
  calSumPitch = calSumRoll = 0.0;
  calSamples = 0;
  setAllMidOn();
  Serial.println("Calibrando (2 s)... mantenha a placa como referência.");
}

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

  float ax = a.acceleration.x;
  float ay = a.acceleration.y;
  float az = a.acceleration.z;

  float rollDeg  = atan2f(ay, az) * 180.0f / PI;
  float pitchDeg = atan2f(-ax, sqrtf(ay*ay + az*az)) * 180.0f / PI;

  if (mode == CALIBRATING) {
    calSumPitch += pitchDeg;
    calSumRoll  += rollDeg;
    calSamples++;

    if (millis() - calStartMs >= CAL_TIME_MS) {
      if (calSamples > 0) {
        pitch0 = calSumPitch / (double)calSamples;
        roll0  = calSumRoll  / (double)calSamples;
      }
      rollFilt  = 0.0f;
      pitchFilt = 0.0f;

      // Pisca todos os LEDs 2x rápido para indicar início
      blinkAllLEDs(2, 150);

      mode = RUNNING;
      Serial.print("Referência fixada: pitch0=");
      Serial.print(pitch0, 2);
      Serial.print(" roll0=");
      Serial.println(roll0, 2);
    }

    delay(10);
    return;
  }

  rollDeg  -= roll0;
  pitchDeg -= pitch0;

  rollFilt  = LPF_ALPHA * rollDeg  + (1.0f - LPF_ALPHA) * rollFilt;
  pitchFilt = LPF_ALPHA * pitchDeg + (1.0f - LPF_ALPHA) * pitchFilt;

  updateCornerLEDs(pitchFilt, rollFilt);

  delay(20); // ~50 Hz
}