agrobot_base/Firmware/Modulos/I2CService.h

#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 Adafruit_MCP23X17 mcp;
    static Adafruit_ADS1115 ads;

    static SemaphoreHandle_t i2cMutex;

    static void DefinirPinos(int sda, int scl) {
        _pinoSDA = sda;
        _pinoSCL = scl;
    }

    static bool IniciarI2C() {
        if (I2CIniciado) {
            Wire.end();
            delay(10);
        }
        I2CIniciado = Wire.begin(_pinoSDA, _pinoSCL);
        //Wire.setTimeout(50);
        PrintTela("[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, 5, &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) {
            PrintTela("[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) {
            PrintTela("[I2C] Solicitacao de acesso negada para o sensor " + String(idSensor) + " - I2C Nao inicializado");
            return false;
        }

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

        // Troca canal se necessário
        if (canalMux >= 0 && !SelecionarSensor(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 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) {
                PrintTela("[I2C] Falha ao endTransmission (leitura)");
                return false;
            }
            delay(1);  // <----- ADICIONAR este delay aqui
        }
        
        size_t received = Wire.requestFrom((int)addr, (int)qtd);
        if (received != qtd || (millis() - t0) > timeoutMs) {
            PrintTela("[I2C] Timeout ou leitura incompleta");
            return false;
        }
        
        for (size_t i = 0; i < qtd; i++) {
            if (Wire.available()) {
                buffer[i] = Wire.read();
            } else {
                PrintTela("[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) {
            PrintTela("[I2C] Falha na escrita ou timeout");
            return false;
        }
    
        return true;
    }


    static bool SelecionarSensor(uint8_t canalGlobal) {
        uint8_t _enderecoMux = 0x70 + (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();
                PrintTela("[MUX] 0x" + String(endereco) + " - Canal alterado de " + String(canalAtivo) + " para " + String(canalMux));
                canalAtivo = canalMux;
            }
            return true;
        }
        PrintTela("[MUX] Nao foi possivel trocar o canal para " + String(canalMux));
        return false;
    }


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

    static void IniciarMCP(byte endereco = enderecoMcp, bool Mandatorio = false) {
        if (!I2CIniciado) {
            PrintTela("[MCP] Impossivel iniciar MCP23X17, I2C nao iniciado!");
            return;
        }
        PrintTela("[MCP] Iniciando MCP23X17...");
        if (!McpIniciado) {
            SolicitarAcessoI2C();
            McpIniciado = mcp.begin_I2C(endereco, &Wire);
            if (!McpIniciado) {
                PrintTela("[MCP] Erro ao iniciar MCP23X17!");
                if (Mandatorio) {
                    PrintTela("[MCP] Conexão obrigatória, tentando novamente em 5 segundos...");
                    delay(5000);
                    IniciarMCP(endereco, Mandatorio);
                }
                else {
                    PrintTela("[MCP] MCP23X17 nao mandatorio, continuando o fluxo...");
                }
            }
            else {
                PrintTela("[MCP] MCP23X17 Iniciado");
            }
            LiberarAcessoI2C();
        }
        else {
            PrintTela("[MCP] MCP23X17 ja Iniciado");
        }
    }
    
    static void IniciarADS(byte endereco = enderecoAds, bool Mandatorio = false) {
        if (!I2CIniciado) {
            PrintTela("[ADS] Impossivel iniciar ADS1115, I2C nao iniciado!");
            return;
        }
        PrintTela("[ADS] Iniciando ADS1115...");
        if (!AdsIniciado) {
            SolicitarAcessoI2C();
            AdsIniciado = ads.begin(endereco, &Wire);
            if (!AdsIniciado) {
                PrintTela("[ADS] Erro ao iniciar ADS1115!");
                if (Mandatorio) {
                    PrintTela("[ADS] Conexão obrigatória, tentando novamente em 5 segundos...");
                    delay(5000);
                    IniciarADS(endereco, Mandatorio);
                }
                else {
                    PrintTela("[ADS] ADS1115 nao mandatorio, continuando o fluxo...");
                }
            }
            else {
                ads.setGain(GAIN_ONE);
                PrintTela("[ADS] ADS1115 Iniciado");
            }
            LiberarAcessoI2C();
        }
        else {
            PrintTela("[ADS] ADS1115 ja Iniciado");
        }
    }
    

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

    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)) {
            PrintTela("[I2C] Semáforo preso pelo ID %d — forçando liberação!\n", idAtualUsandoI2C);
            LiberarAcessoI2C(idAtualUsandoI2C);
            IniciarI2C();
        }
    }

};

int I2CService::_pinoSDA = 1;
int I2CService::_pinoSCL = 2;
unsigned long I2CService::LimiteTempoI2C = 100;
bool I2CService::I2CIniciado = false;
bool I2CService::MuxIniciado = false;
bool I2CService::McpIniciado = false;
Adafruit_MCP23X17 I2CService::mcp;
Adafruit_ADS1115 I2CService::ads;
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;

#endif