#ifndef I2CService_h
#define I2CService_h

#include "SerialService.h"
#include <Wire.h>
#include <Adafruit_MCP23X17.h>
#include <Adafruit_ADS1X15.h>
#include <freertos/semphr.h>

class I2CService {
  public:
    static const constexpr uint8_t enderecoMux = 0x70;
    static const constexpr uint8_t enderecoMcp = 0x20;
    static const constexpr uint8_t enderecoAds = 0x48;
    static bool I2CIniciado;
    static bool MuxIniciado;
    static bool McpIniciado;
    static bool AdsIniciado;
    static int canalAtivo;
    static int idAtualUsandoI2C;
    static bool i2cMutexReiniciando;

    static Adafruit_MCP23X17 mcp;
    static Adafruit_ADS1115 ads;

    static bool DefinirPinos(int sda, int scl) {
        bool mudou = false;
        if (_pinoSDA != sda) {
            _pinoSDA = sda;
            mudou = true;
        }
        if (_pinoSCL != scl) {
            _pinoSCL = scl;
            mudou = true;
        }
        return mudou;
    }

    static bool IniciarI2C() {
        bool wireLiberado = WirePodeFinalizar();
        if (I2CIniciado) {
            MostrarLog("I2C", "Tentando finalizar Wire...");
            if (wireLiberado) {
                Wire.end();
                vTaskDelay(10);
                MostrarLog("I2C", "Wire finalizado com sucesso");
            } else {
                MostrarLog("I2C", "⚠️ Wire travado (SDA/SCL em LOW), nao foi finalizado");
            }
        }
        if (wireLiberado) {
            MostrarLog("I2C", "Iniciando Wire...");
            I2CIniciado = Wire.begin(_pinoSDA, _pinoSCL);
            //Wire.setTimeout(50);
            MostrarLog("I2C", "I2C inicializado nos pinos SDA=" + String(_pinoSDA) + " e SCL=" + String(_pinoSCL) + " - Res " + I2CIniciado);
        }
        if (i2cMutex == nullptr) {
            i2cMutex = xSemaphoreCreateMutex();
        }
        idAtualUsandoI2C = -1;
        tempoEntradaI2C = 0;

        if (i2cTaskHandle == NULL) {
            xTaskCreatePinnedToCore(I2CService::i2cTaskWrapper, "i2cTask", 4096, nullptr, 2, &i2cTaskHandle, tskNO_AFFINITY);
        }

        return I2CIniciado;
    }

    static bool VerificaEnderecoBarramento(byte _Endereco, uint32_t timeoutMs = 50) {
        if (!I2CIniciado) return false;
    
        //Wire.setTimeout(timeoutMs);
        unsigned long t0 = millis();
    
        Wire.beginTransmission(_Endereco);
        byte error = Wire.endTransmission();
    
        if ((millis() - t0) > timeoutMs) {
            MostrarLog("I2C", "Timeout verificando endereco " + String(_Endereco));
            return false;
        }
    
        return error == 0;
    }

    static bool SolicitarAcessoI2C(int idSensor = 0, int canalMux = -1, uint32_t timeoutMs = 200) {
        if (!I2CIniciado) {
            MostrarLog("I2C", "Solicitacao de acesso negada para o sensor " + String(idSensor) + " - I2C Nao inicializado");
            return false;
        }

        if (i2cMutexReiniciando) return false;

        TickType_t timeoutTicks = pdMS_TO_TICKS(timeoutMs);
        if (xSemaphoreTake(i2cMutex, timeoutTicks) != pdTRUE) {
            MostrarLog("I2C", "Timeout ao tentar acessar o I2C - ID: " + String(idSensor));
            return false;
        }

        // Troca canal se necessário
        if (canalMux > -1 && !SelecionarCanalMux(canalMux)) {
            xSemaphoreGive(i2cMutex);  // Libera caso a troca de canal falhe
            return false;
        }

        idAtualUsandoI2C = idSensor;
        tempoEntradaI2C = millis();

        return true;
    }

    static void LiberarAcessoI2C(int idSensor = 0) {
        if (idSensor == idAtualUsandoI2C) {
            idAtualUsandoI2C = -1;
            xSemaphoreGive(i2cMutex);
        }
    }

    static void RecriarMutex() {
        i2cMutexReiniciando = true;
        vSemaphoreDelete(i2cMutex);
        i2cMutex = xSemaphoreCreateMutex();
        xSemaphoreGive(i2cMutex);
        idAtualUsandoI2C = -1;
        i2cMutexReiniciando = false;
        MostrarLog("I2C", "Mutex Recriado");
    }

    static bool WirePodeFinalizar() {
        pinMode(_pinoSCL, INPUT_PULLUP);
        pinMode(_pinoSDA, INPUT_PULLUP);
        delay(1);  // garante leitura estável
        return (digitalRead(_pinoSCL) == HIGH && digitalRead(_pinoSDA) == HIGH);
    }

    static void DestravarBarramentoI2C(int sda, int scl) {
        pinMode(scl, OUTPUT_OPEN_DRAIN);
        pinMode(sda, INPUT_PULLUP);  // SDA como entrada para ler

        for (int i = 0; i < 9; i++) {
            digitalWrite(scl, LOW);
            delayMicroseconds(5);
            digitalWrite(scl, HIGH);
            delayMicroseconds(5);
        }

        // Gera uma condição de STOP manual (SDA sobe enquanto SCL está alto)
        pinMode(sda, OUTPUT_OPEN_DRAIN);
        digitalWrite(sda, LOW);
        delayMicroseconds(5);
        digitalWrite(scl, HIGH);
        delayMicroseconds(5);
        digitalWrite(sda, HIGH);

        // Espera estabilizar
        delay(5);

        // Retorna controle ao Wire
        pinMode(scl, INPUT_PULLUP);
        pinMode(sda, INPUT_PULLUP);
    }

    static int QuemEstaUsando() {
        return idAtualUsandoI2C;
    }

    static bool LeituraSegura(uint8_t addr, uint8_t* buffer, size_t qtd, uint8_t reg = 0xFF, uint32_t timeoutMs = 50) {
        //Wire.setTimeout(timeoutMs);
        unsigned long t0 = millis();
        
        if (reg != 0xFF) {
            Wire.beginTransmission(addr);
            Wire.write(reg);
            if (Wire.endTransmission(false) != 0) {
                MostrarLog("I2C", "Falha ao endTransmission (leitura)");
                return false;
            }
            vTaskDelay(1);  // <----- ADICIONAR este delay aqui
        }
        
        size_t received = Wire.requestFrom((int)addr, (int)qtd);
        if (received != qtd || (millis() - t0) > timeoutMs) {
            MostrarLog("I2C", "Timeout ou leitura incompleta");
            return false;
        }
        
        for (size_t i = 0; i < qtd; i++) {
            if (Wire.available()) {
                buffer[i] = Wire.read();
            } else {
                MostrarLog("I2C", "Dados insuficientes disponíveis");
                return false;
            }
        }
        
        return true;
    }    
    
    static bool EscritaSegura(uint8_t addr, const uint8_t* dados, size_t qtd, uint32_t timeoutMs = 50) {
        //Wire.setTimeout(timeoutMs);
        unsigned long t0 = millis();
    
        Wire.beginTransmission(addr);
        for (size_t i = 0; i < qtd; i++) {
            Wire.write(dados[i]);
        }
    
        int resultado = Wire.endTransmission();
        if ((millis() - t0) > timeoutMs || resultado != 0) {
            MostrarLog("I2C", "Falha na escrita ou timeout");
            return false;
        }
    
        return true;
    }


    static bool SelecionarCanalMux(uint8_t canalGlobal) {
        uint8_t _enderecoMux = enderecoMux + (canalGlobal / 10);
        uint8_t _canal = canalGlobal % 10;
        return TrocarCanalMux(_enderecoMux, _canal);
    }

    static bool TrocarCanalMux(uint8_t endereco, uint8_t canalMux) {
        if (MuxIniciado && canalMux >= 0 && canalMux < 9) {
            if (canalMux != canalAtivo) {
                Wire.beginTransmission(endereco);
                Wire.write(1 << canalMux);
                Wire.endTransmission();
                MostrarLog("MUX", "0x" + String(endereco) + " - Canal alterado de " + String(canalAtivo) + " para " + String(canalMux));
                canalAtivo = canalMux;
            }
            return true;
        }
        MostrarLog("MUX", "Nao foi possivel trocar o canal para " + String(canalMux));
        return false;
    }


    static int SelecionarCanalADS(uint8_t canalGlobal) {
        if (canalGlobal > 33) return -1;
        uint8_t endereco = enderecoAds + (canalGlobal / 10);
        uint8_t canal = canalGlobal % 10;
        if (IniciarADS(endereco)) {
            return canal;
        }
        else {
            return -1;
        }
    }

    static int RealizarLeituraADS(int canalGlobal, int leituras, int idSensor, int canalMux) {
        SolicitarAcessoI2C(idSensor, canalMux);
        int canal = SelecionarCanalADS(canalGlobal);
        if (canal > -1) {
            uint16_t l = 0;
            for (int i = 0; i < leituras; i++) {
                vTaskDelay(1);
                int leituraADC = (int)(ads.readADC_SingleEnded(canal));
                int leitura12bits = map(leituraADC, 0, 32767, 0, 4095);
                MostrarLog("ADS", "Leitura: " + String(leituraADC) + ", Leitura 12 bits: " + String(leitura12bits));
                l += leitura12bits;
            }
            LiberarAcessoI2C(idSensor);
            return (int)(l / leituras);
        }
        LiberarAcessoI2C(idSensor);
        return -1;
    }


    static void IniciarMUX(byte endereco = enderecoMux, bool Mandatorio = false) {
        if (!I2CIniciado) {
            MostrarLog("MUX", "Impossivel iniciar TCA9548A, I2C nao iniciado!");
            return;
        }
        MostrarLog("MUX", "Iniciando TCA9548A...");
        if (!MuxIniciado) {
            SolicitarAcessoI2C();
            MuxIniciado = VerificaEnderecoBarramento(endereco);
            if (!MuxIniciado) {
                MostrarLog("MUX", "TCA9548A nao Iniciado, endereco " + String(endereco) + " nao encontrado no barramento");
                if (Mandatorio) {
                    MostrarLog("MUX", "Conexão obrigatória, tentando novamente em 5 segundos...");
                    vTaskDelay(5000);
                    IniciarMUX(endereco, Mandatorio);
                }
                else {
                    MostrarLog("MUX", "TCA9548A nao mandatorio, continuando o fluxo...");
                }
            }
            else {
                canalAtivo = -1;  // Nenhum canal ativo ainda
                idAtualUsandoI2C = -1;
                i2cMutex = xSemaphoreCreateMutex();
                MostrarLog("MUX", "TCA9548A Iniciado");
            }
            LiberarAcessoI2C();
        }
        else {
            MostrarLog("MUX", "TCA9548A ja Iniciado");
        }
    }

    static void IniciarMCP(byte endereco = enderecoMcp, bool Mandatorio = false) {
        if (!I2CIniciado) {
            MostrarLog("MCP", "Impossivel iniciar MCP23X17, I2C nao iniciado!");
            return;
        }
        MostrarLog("MCP", "Iniciando MCP23X17...");
        if (!McpIniciado) {
            SolicitarAcessoI2C();
            McpIniciado = mcp.begin_I2C(endereco, &Wire);
            if (!McpIniciado) {
                MostrarLog("MCP", "Erro ao iniciar MCP23X17!");
                if (Mandatorio) {
                    MostrarLog("MCP", "Conexão obrigatória, tentando novamente em 5 segundos...");
                    vTaskDelay(5000);
                    IniciarMCP(endereco, Mandatorio);
                }
                else {
                    MostrarLog("MCP", "MCP23X17 nao mandatorio, continuando o fluxo...");
                }
            }
            else {
                MostrarLog("MCP", "MCP23X17 Iniciado");
            }
            LiberarAcessoI2C();
        }
        else {
            MostrarLog("MCP", "MCP23X17 ja Iniciado");
        }
    }
    
    static bool IniciarADS(byte endereco = enderecoAds) {
        if (!I2CIniciado) {
            MostrarLog("ADS", "Impossivel iniciar ADS1115, I2C nao iniciado!");
            return false;
        }
        if (AdsIniciado && endereco == ultimoEnderecoADS) {
            MostrarLog("ADS", "ADS1115 já iniciado no endereco 0x" + String(endereco, HEX));
            return true;
        }
        MostrarLog("ADS", "Iniciando ADS1115 no endereco 0x" + String(endereco, HEX) + "...");
        AdsIniciado = ads.begin(endereco, &Wire);
        if (!AdsIniciado) {
            ultimoEnderecoADS = 0xFF;
            MostrarLog("ADS", "Erro ao iniciar ADS1115 no endereco 0x" + String(endereco, HEX));
        }
        else {
            ads.setGain(GAIN_ONE);
            MostrarLog("ADS", "ADS1115 Iniciado no endereco 0x" + String(endereco, HEX));
            ultimoEnderecoADS = endereco;
        }
        return AdsIniciado;
    }
    

  private:
    static bool DebugMode;

    static void MostrarLog(String Componente, String mensagem) {
        if (DebugMode) {
            PrintTela("[" + Componente +  "] " + mensagem);
        }
    }

    static int _pinoSDA;
    static int _pinoSCL;
    static unsigned long tempoEntradaI2C;
    static unsigned long LimiteTempoI2C;

    static byte ultimoEnderecoADS;
    static SemaphoreHandle_t i2cMutex;

    static TaskHandle_t i2cTaskHandle;
    static void i2cTaskWrapper(void *pvParameters) {
      I2CService::i2cTask();
    }

    static void i2cTask() {
        while (true) {
            VerificarI2CPreso();
            vTaskDelay(1);
        }
    }

    static void VerificarI2CPreso() {
        if (idAtualUsandoI2C >= 0 && (millis() - tempoEntradaI2C > LimiteTempoI2C)) {
            MostrarLog("I2C", "Semáforo preso pelo ID " + String(idAtualUsandoI2C) + " — forçando liberação!");
            ESP.restart();
            //RecriarMutex();
            //DestravarBarramentoI2C(_pinoSDA, _pinoSCL);
            //IniciarI2C();
        }
    }

};

bool I2CService::DebugMode = false;
int I2CService::_pinoSDA = 1;
int I2CService::_pinoSCL = 2;
unsigned long I2CService::LimiteTempoI2C = 2000;
bool I2CService::I2CIniciado = false;
bool I2CService::MuxIniciado = false;
bool I2CService::McpIniciado = false;
Adafruit_MCP23X17 I2CService::mcp;
Adafruit_ADS1115 I2CService::ads;
byte I2CService::ultimoEnderecoADS = 0xFF;
bool I2CService::AdsIniciado = false;
int I2CService::canalAtivo = -1;
int I2CService::idAtualUsandoI2C = -1;
unsigned long I2CService::tempoEntradaI2C = 0;
SemaphoreHandle_t I2CService::i2cMutex = nullptr;
TaskHandle_t I2CService::i2cTaskHandle = NULL;
bool I2CService::i2cMutexReiniciando = false;

#endif