#include <Wire.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include <Adafruit_ADS1X15.h>
#include <Adafruit_INA219.h>

// ======================================================
// CONFIGURAÇÕES
// ======================================================

// -------- Pinos --------
static const int PIN_IGBT_COND_SENSE = 34;   // HIGH = IGBT conduzindo
static const int PIN_CURRENT_ENABLE  = 27;   // habilita fonte de corrente
static const int PIN_REF_DAC         = 25;   // opcional, ESP32 clássico

// -------- Parâmetros do teste --------
float Iref_A = 0.600f;              // corrente desejada
float Vref_V = 0.600f;              // 1 ohm -> 600 mA = 0,600 V

const uint32_t TEST_DURATION_MS = 1000;
const uint32_t SAMPLE_PERIOD_MS = 20;    // 50 amostras/s
const uint32_t MONITOR_PERIOD_MS = 250;  // monitor BLE
const size_t MAX_SAMPLES = TEST_DURATION_MS / SAMPLE_PERIOD_MS;

// -------- Correção do INA219 --------
// Icorrigida = ganho * Iina + offset
float inaGainCorr   = 1.0000f;
float inaOffsetCorr = 0.0000f; // em A

// -------- BLE UUIDs --------
#define SERVICE_UUID        "7e8b1000-0001-4b2e-a9c7-1234567890ab"
#define CHAR_CMD_UUID       "7e8b1001-0001-4b2e-a9c7-1234567890ab"
#define CHAR_STATUS_UUID    "7e8b1002-0001-4b2e-a9c7-1234567890ab"
#define CHAR_MONITOR_UUID   "7e8b1003-0001-4b2e-a9c7-1234567890ab"
#define CHAR_SUMMARY_UUID   "7e8b1004-0001-4b2e-a9c7-1234567890ab"
#define CHAR_SAMPLE_UUID    "7e8b1005-0001-4b2e-a9c7-1234567890ab"

// ======================================================
// OBJETOS GLOBAIS
// ======================================================

Adafruit_ADS1115 ads;
Adafruit_INA219 ina219;

BLEServer* pServer = nullptr;
BLECharacteristic* chCmd = nullptr;
BLECharacteristic* chStatus = nullptr;
BLECharacteristic* chMonitor = nullptr;
BLECharacteristic* chSummary = nullptr;
BLECharacteristic* chSample = nullptr;

bool bleConnected = false;

// controle do teste
bool testRunning = false;
bool startRequested = false;
bool stopRequested = false;

// buffers do último ensaio
float icBuffer[MAX_SAMPLES];
float vceBuffer[MAX_SAMPLES];
uint32_t tBuffer[MAX_SAMPLES];
size_t sampleCount = 0;

// estatísticas
float icMin = 0.0f, icMax = 0.0f, icMean = 0.0f;
float vceMin = 0.0f, vceMax = 0.0f, vceMean = 0.0f;

// ======================================================
// AUXILIARES BLE
// ======================================================

void notifyString(BLECharacteristic* ch, const String& s) {
  ch->setValue(s.c_str());
  if (bleConnected) ch->notify();
}

void setStatus(const String& s) {
  Serial.println("[STATUS] " + s);
  notifyString(chStatus, s);
}

// ======================================================
// LEITURAS
// ======================================================

// leitura média do INA219 para validar corrente real
float readIc_A() {
  const int N = 4;
  float sum = 0.0f;

  for (int i = 0; i < N; i++) {
    float current_mA = ina219.getCurrent_mA();
    sum += current_mA / 1000.0f;
    delay(1);
  }

  float iA = sum / N;
  float iCorr = (inaGainCorr * iA) + inaOffsetCorr;
  return iCorr;
}

// tensão Vce medida pelo ADS1115
float readVce_V() {
  int16_t raw = ads.readADC_SingleEnded(0);
  return ads.computeVolts(raw);
}

bool igbtIsConducting() {
  return digitalRead(PIN_IGBT_COND_SENSE) == HIGH;
}

void setCurrentSource(bool enable) {
  digitalWrite(PIN_CURRENT_ENABLE, enable ? HIGH : LOW);

  // Se quiser usar DAC real no futuro:
  // int dacValue = (int)((Vref_V / 3.3f) * 255.0f);
  // dacValue = constrain(dacValue, 0, 255);
  // if (enable) dacWrite(PIN_REF_DAC, dacValue);
  // else dacWrite(PIN_REF_DAC, 0);
}

void updateMonitor(float ic, float vce, bool cond, bool srcEnabled) {
  String payload;
  payload.reserve(96);
  payload += "Iref_A=" + String(Iref_A, 3);
  payload += ";Iina_A=" + String(ic, 3);
  payload += ";Vce_V=" + String(vce, 3);
  payload += ";COND=" + String(cond ? 1 : 0);
  payload += ";SRC=" + String(srcEnabled ? 1 : 0);
  notifyString(chMonitor, payload);
}

void updateSummary() {
  String payload;
  payload.reserve(128);
  payload += "Ic_mean=" + String(icMean, 4);
  payload += ";Ic_min=" + String(icMin, 4);
  payload += ";Ic_max=" + String(icMax, 4);
  payload += ";Vce_mean=" + String(vceMean, 4);
  payload += ";Vce_min=" + String(vceMin, 4);
  payload += ";Vce_max=" + String(vceMax, 4);
  payload += ";N=" + String(sampleCount);
  notifyString(chSummary, payload);
}

// ======================================================
// ESTATÍSTICA
// ======================================================

void resetStats() {
  sampleCount = 0;
  icMin = 999999.0f;
  icMax = -999999.0f;
  icMean = 0.0f;
  vceMin = 999999.0f;
  vceMax = -999999.0f;
  vceMean = 0.0f;
}

void finalizeStats() {
  if (sampleCount == 0) {
    icMin = icMax = icMean = 0.0f;
    vceMin = vceMax = vceMean = 0.0f;
    return;
  }

  float sumIc = 0.0f;
  float sumVce = 0.0f;

  for (size_t i = 0; i < sampleCount; i++) {
    float ic = icBuffer[i];
    float vce = vceBuffer[i];

    if (ic < icMin) icMin = ic;
    if (ic > icMax) icMax = ic;
    if (vce < vceMin) vceMin = vce;
    if (vce > vceMax) vceMax = vce;

    sumIc += ic;
    sumVce += vce;
  }

  icMean = sumIc / sampleCount;
  vceMean = sumVce / sampleCount;
}

// ======================================================
// TESTE
// ======================================================

void runOneSecondCapture() {
  if (testRunning) return;

  testRunning = true;
  stopRequested = false;
  resetStats();

  setStatus("WAIT_IGBT_ON");

  bool cond = igbtIsConducting();
  float icNow = readIc_A();
  float vceNow = readVce_V();
  updateMonitor(icNow, vceNow, cond, false);

  if (!cond) {
    setCurrentSource(false);
    setStatus("ERR_IGBT_OFF");
    testRunning = false;
    return;
  }

  setStatus("CAPTURING");
  setCurrentSource(true);
  delay(20);

  uint32_t t0 = millis();
  uint32_t nextSample = t0;
  uint32_t nextMonitor = t0;

  while ((millis() - t0) < TEST_DURATION_MS) {
    if (stopRequested) {
      setCurrentSource(false);
      setStatus("STOPPED");
      finalizeStats();
      updateSummary();
      testRunning = false;
      return;
    }

    uint32_t now = millis();

    if (now >= nextSample && sampleCount < MAX_SAMPLES) {
      float ic = readIc_A();
      float vce = readVce_V();
      bool condNow = igbtIsConducting();

      if (!condNow) {
        setCurrentSource(false);
        setStatus("ERR_IGBT_OFF_DURING_TEST");
        finalizeStats();
        updateSummary();
        testRunning = false;
        return;
      }

      icBuffer[sampleCount] = ic;
      vceBuffer[sampleCount] = vce;
      tBuffer[sampleCount] = now - t0;

      String sampleLine;
      sampleLine.reserve(64);
      sampleLine += "t_ms=" + String(tBuffer[sampleCount]);
      sampleLine += ";Ic_A=" + String(ic, 4);
      sampleLine += ";Vce_V=" + String(vce, 4);
      notifyString(chSample, sampleLine);

      sampleCount++;
      nextSample += SAMPLE_PERIOD_MS;
    }

    if (now >= nextMonitor) {
      float ic = (sampleCount > 0) ? icBuffer[sampleCount - 1] : icNow;
      float vce = (sampleCount > 0) ? vceBuffer[sampleCount - 1] : vceNow;
      updateMonitor(ic, vce, true, true);
      nextMonitor += MONITOR_PERIOD_MS;
    }

    delay(1);
  }

  setCurrentSource(false);
  finalizeStats();
  updateSummary();
  setStatus("DONE");
  testRunning = false;
}

// ======================================================
// COMANDOS
// ======================================================

void processCommand(String cmd) {
  cmd.trim();
  Serial.println("[CMD] " + cmd);

  String uc = cmd;
  uc.toUpperCase();

  if (uc == "START") {
    startRequested = true;
    setStatus("START_REQ");
    return;
  }

  if (uc == "STOP") {
    stopRequested = true;
    return;
  }

  if (uc == "READ") {
    float ic = readIc_A();
    float vce = readVce_V();
    bool cond = igbtIsConducting();
    updateMonitor(ic, vce, cond, false);
    updateSummary();
    setStatus("READ_OK");
    return;
  }

  if (uc.startsWith("SETI=")) {
    String valueStr = cmd.substring(5);
    float newI = valueStr.toFloat();

    if (newI > 0.0f && newI <= 1.0f) {
      Iref_A = newI;
      Vref_V = Iref_A * 1.0f;  // 1 ohm
      setStatus("SETI_OK");
      updateMonitor(readIc_A(), readVce_V(), igbtIsConducting(), false);
    } else {
      setStatus("ERR_SETI_RANGE");
    }
    return;
  }

  // formato: SETCAL=1.002,-0.003
  if (uc.startsWith("SETCAL=")) {
    String params = cmd.substring(7);
    int comma = params.indexOf(',');

    if (comma > 0) {
      float g = params.substring(0, comma).toFloat();
      float o = params.substring(comma + 1).toFloat();

      if (g > 0.5f && g < 1.5f) {
        inaGainCorr = g;
        inaOffsetCorr = o;
        setStatus("SETCAL_OK");
      } else {
        setStatus("ERR_SETCAL_RANGE");
      }
    } else {
      setStatus("ERR_SETCAL_FMT");
    }
    return;
  }

  if (uc == "GETCAL") {
    String payload;
    payload += "gain=" + String(inaGainCorr, 6);
    payload += ";offset=" + String(inaOffsetCorr, 6);
    notifyString(chSummary, payload);
    setStatus("GETCAL_OK");
    return;
  }

  setStatus("ERR_UNKNOWN_CMD");
}

// ======================================================
// CALLBACKS BLE
// ======================================================

class MyServerCallbacks : public BLEServerCallbacks {
  void onConnect(BLEServer* pServer) override {
    bleConnected = true;
    Serial.println("BLE conectado");
    setStatus("CONNECTED");
  }

  void onDisconnect(BLEServer* pServer) override {
    bleConnected = false;
    Serial.println("BLE desconectado");
    BLEDevice::startAdvertising();
  }
};

class CmdCallbacks : public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic* pCharacteristic) override {
    std::string rx = pCharacteristic->getValue();
    if (!rx.empty()) {
      processCommand(String(rx.c_str()));
    }
  }
};

// ======================================================
// BLE
// ======================================================

void setupBLE() {
  BLEDevice::init("ESP32_IGBT_TEST");

  pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());

  BLEService* service = pServer->createService(SERVICE_UUID);

  chCmd = service->createCharacteristic(
    CHAR_CMD_UUID,
    BLECharacteristic::PROPERTY_WRITE | BLECharacteristic::PROPERTY_READ
  );

  chStatus = service->createCharacteristic(
    CHAR_STATUS_UUID,
    BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY
  );

  chMonitor = service->createCharacteristic(
    CHAR_MONITOR_UUID,
    BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY
  );

  chSummary = service->createCharacteristic(
    CHAR_SUMMARY_UUID,
    BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY
  );

  chSample = service->createCharacteristic(
    CHAR_SAMPLE_UUID,
    BLECharacteristic::PROPERTY_NOTIFY
  );

  chCmd->setCallbacks(new CmdCallbacks());

  chStatus->addDescriptor(new BLE2902());
  chMonitor->addDescriptor(new BLE2902());
  chSummary->addDescriptor(new BLE2902());
  chSample->addDescriptor(new BLE2902());

  chStatus->setValue("BOOT");
  chMonitor->setValue("Iref_A=0.600;Iina_A=0.000;Vce_V=0.000;COND=0;SRC=0");
  chSummary->setValue("Ic_mean=0;Ic_min=0;Ic_max=0;Vce_mean=0;Vce_min=0;Vce_max=0;N=0");
  chCmd->setValue("READY");

  service->start();

  BLEAdvertising* advertising = BLEDevice::getAdvertising();
  advertising->addServiceUUID(SERVICE_UUID);
  advertising->setScanResponse(true);
  advertising->setMinPreferred(0x06);
  advertising->setMinPreferred(0x12);

  BLEDevice::startAdvertising();
  Serial.println("BLE advertising iniciado");
}

// ======================================================
// SETUP / LOOP
// ======================================================

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

  pinMode(PIN_IGBT_COND_SENSE, INPUT);
  pinMode(PIN_CURRENT_ENABLE, OUTPUT);
  digitalWrite(PIN_CURRENT_ENABLE, LOW);

  Wire.begin();

  if (!ina219.begin()) {
    Serial.println("Erro ao iniciar INA219");
  } else {
    // Para módulo padrão com shunt típico
    ina219.setCalibration_32V_1A();
    Serial.println("INA219 OK");
  }

  if (!ads.begin()) {
    Serial.println("Erro ao iniciar ADS1115");
  } else {
    ads.setGain(GAIN_TWO); // ~1 V com boa resolução
    Serial.println("ADS1115 OK");
  }

  setupBLE();
  setStatus("IDLE");
}

void loop() {
  static uint32_t lastMonitor = 0;
  uint32_t now = millis();

  if (startRequested && !testRunning) {
    startRequested = false;
    runOneSecondCapture();
  }

  if (!testRunning && (now - lastMonitor >= MONITOR_PERIOD_MS)) {
    float ic = readIc_A();
    float vce = readVce_V();
    bool cond = igbtIsConducting();
    updateMonitor(ic, vce, cond, false);
    lastMonitor = now;
  }

  delay(5);
}