#include <Arduino.h>
#include <FastLED.h>
#include "FS.h"
#include "SPIFFS.h"
#include <Preferences.h>
// #include "nvs_flash.h"
// #include "nvs.h"
#include <SPI.h>
#include <curveFitting.h>
#include <math.h>
#include <HardwareSerial.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include <mbedtls/md.h>
#include <esp_system.h>
#include "driver/uart.h" // include uart driver headers
#include "esp_vfs_dev.h" // include esp_vfs_dev headers
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include "esp_log.h"             // Define a TAG for your logs
#include "SpeedEstimator.h"      // The public functions for the speed estimator
#include "PWMCapture.h"          // Define a TAG for your logs
static const char *TAG = "MAIN"; // add to check

// ===================================================================================
//
//                               5. FUNCTION PROTOTYPES
//
// ===================================================================================
bool setValue(uint8_t *value, size_t length);
void readAnalogInputs();
float arredondarParaMultiploDe50(float peso);
float interpolateResidualHeight(float pesoKg, int cc);
float interpolateResidualWeight(float alturaMm, int cc);
void handlePWMState();
void processPesoEmMovimento();
void processPesoEstabilizado();
float lagrange4pts(double x, double *x_vals, double *y_vals);
void findClosest4Points(double x, const double *x_vals, const double *y_vals, int n, double out_x[4], double out_y[4]);
float corrigirErroSensor(float sensor);
float calculateWeightSpecial(float sensorValue);
float calculateWeightSpecialStage2(float sensorValue);
float calculateWeightFinal();
void detectarCargaInstantanea();
void congelarPesoPorCargaComDelay();
float convertMvToBar(float mv);
float getKspByPressure(float p);
float calculateStaticWeight(float mv);
float calculateDynamicWeight(float pBar, float velMmPs);
float calcularVelocidade();
void wavePattern(CRGB color, int speed, bool direction, bool enableWaveEffect);
void updateLedVisual(float pesoAtual);
void piscarLedVinho();
void BluetoothLedAzul();
void checkLiftStage();
void IRAM_ATTR encoderA();
void IRAM_ATTR encoderB();
void IRAM_ATTR encoderISR();
void updatePositionMm();
void updateElevacaoParada();
void applySafetyRestrictions(float pesoAtual);
void handleForkLogic();
void applyFinalRelayState();
void buzzerControl();
void exibirStatus(float pesoAtual, float duty_cycle, float estimated_speed);
void printQuandoCarregar();
void sendBLE(BLECharacteristic *characteristic, const String &message);
void logMessage(String msg);
void bluetoothConnection();
void bluetoothReceiveData();
void atualizarReles(float pesoAtual);
void sendResidualTable();
void saveResidualTableToPreferences();
void loadResidualTableFromPreferences();
void processarComando(String value);
void bluetoothTimeOut(unsigned long currentTime);
void inicializarTabelaSeNecessario();

// =========================== PINOS ESP32 ===========================
#define CS1_PIN 9  // Chip Select para o DAC
#define CS0_PIN 10 // Chip Select para o ADC
#define MOSI_PIN 11
#define MISO_PIN 13
#define SCLK_PIN 12
#define BUZZER_PIN 37
#define RELAY1_PIN 38 // Relé Tração
#define RELAY2_PIN 39 // Relé Elevação
#define IND_NA 4
#define IND_NF 5
#define BIESTAVEL 6
#define ENT_RES 7
#define ENC_B 1
#define ENC_A 2
#define PWM_FREIO 42
#define PWM_ELEV 41
#define SAIDA_RES 17
#define PINO_5V 47
#define PINO_12V 48

static bool primeiraInicializacao = true;
bool inSecondStage = false;
bool emTransicaoDeEstagio = false;
float pesoCongelado2Segundos = NAN;
float pesoSeguro = NAN;
float valorPesoCongelado = NAN;
unsigned long tempoZerarPeso = 0;
const unsigned long TEMPO_PARA_ZERAR_PESO_MS = 500;

unsigned long tempoZerarPeso = 0;
const unsigned long TEMPO_PARA_ZERAR_PESO_MS = 500;

// =========================== ENUM'S ===========================
typedef enum
{
    TIPO_TORRE_DUPLEX = 0,
    TIPO_TORRE_TRIPLEX
} tipo_torre_t;

typedef enum
{
    BUZZER_OFF = 0,
    BUZZER_BEEP_SHORT,
    BUZZER_BEEP_CONTINUOUS
} buzzer_state_t;

// =========================== 1 - PESO ===========================
// =========================== SENSOR DE PESO ===========================
float weightSensorValue = 0.0;
const int BUFFER_SIZE = 100;
uint16_t adcBuffer[BUFFER_SIZE] = {0};
uint16_t bufferIndex = 0;
bool bufferFilled = false;
float pesoSensorOffset = 0.0;

// ===========================  FILTRAGEM E ARMAZENAMENTO DE PESO ===========================
float ultimoPesoValido = 0.0;
float pesoSuavizado = 300.0; // Começa em 300kg
float pesoAtual = NAN;
bool primeiroPeso = true;
volatile unsigned long lastTimeAnalogRead = 0;
const float ALPHA = 0.05;

// =========================== PESO ESTABILIZADO  ===========================
float pesoEstabilizado = NAN;
unsigned long tempoEstabilizacao = 0;
const int TEMPO_HOLD = 300;
int MARGEM_SEGURANCA_MM = 100;

// =========================== PESO EM MOVIMENTO  ===========================
float pesoMoviAtual = 0.0;
float pesoMoviCongelado = NAN;
float bufferMovimento[10] = {0};
int indexMovimento = 0;
bool coletandoMovimento = false;
bool pesoMoviJaCongelado = false;

// --- REGRAS DE CONGELAMENTO DE PESO ---
const float LIMIAR_ZERO = 10.0f;   // ~zero → destrava
const float TOL_PARECIDO = 100.0f; // ±50 kg = “parecido”, ajustável
const unsigned long HOLD_MS_NOVO = TEMPO_HOLD;

// =========================== CONGELAMENTO AUTOMÁTICO POR CARGA ===========================
float pesoCongeladoPorCarga = NAN;

bool cargaDetectada = false;
float pesoAnterior = 0.0;
const float LIMIAR_CARGA = 1000.0;
bool podeCongelarPeso = true;

unsigned long tempoInicioBloqueio = 0;
const unsigned long TEMPO_BLOQUEIO_MS = 2000;

bool pesoJaCongelado = false;

// =========================== INTERPOLAÇÃO E CÁLCULO FINAL ===========================
float currentWeight = 0;
float smoothedWeight = 0.0;
const float PESO_SEGUNDO_ESTAGIO = 0.0;

float teachIn_P1_stage1;
float teachIn_P2_stage1;
float teachIn_P3_stage1;
float teachIn_V1_stage1;
float teachIn_V2_stage1;
float teachIn_V3_stage1;

float teachIn_P1_stage2;
float teachIn_P2_stage2;
float teachIn_P3_stage2;
float teachIn_V1_stage2;
float teachIn_V2_stage2;
float teachIn_V3_stage2;

// ------------------- CURVAS DE CALIBRAÇÃO -------------------
const double V1_AD[] = {620, 857, 872, 927, 1010, 1190, 1236, 1279, 1380, 1485, 1544, 1993, 2350, 2400};
const double WT_STAGE1[] = {0, 200, 250, 300, 350, 500, 570, 600, 700, 800, 850, 1282, 1554, 1604, 1654};
const double V2_AD[] = {940, 1213, 1290, 1335, 1430, 1620, 1725, 1900, 2025, 2090, 3000, 3150, 3200};
const double WT_STAGE2[] = {0, 200, 250, 300, 350, 500, 570, 700, 800, 850, 1282, 1554, 1604};
const double V_SPECIAL[] = {830, 1016, 1085, 1105, 1410, 1800, 2500, 2530, 2540};
const double WT_SPECIAL[] = {0, 232, 250, 260, 570, 900, 1554, 1604, 1650};
const double V_SPECIAL_STAGE2[] = {1000, 1750, 1780, 3100, 3250, 3280};
const double WT_SPECIAL_STAGE2[] = {0, 550, 570, 1554, 1604, 1654};

#define ARRAY_SIZE_SPECIAL_STAGE2 (sizeof(V_SPECIAL_STAGE2) / sizeof(double))
#define ARRAY_SIZE_SPECIAL (sizeof(V_SPECIAL) / sizeof(double))
#define ARRAY_SIZE_STAGE1 (sizeof(V1_AD) / sizeof(double))
#define ARRAY_SIZE_STAGE2 (sizeof(V2_AD) / sizeof(double))

// ------------------- TABELA RESIDUAL -------------------
#define RESIDUAL_ROWS 7
#define RESIDUAL_COLS 5
const int RESIDUAL_ARRAY_SIZE = 7;
int centroCarga;

const int ccValues[RESIDUAL_COLS] = {600, 650, 700, 750, 900};
int weightResidual[RESIDUAL_ROWS][RESIDUAL_COLS] = {
    {600, 531, 493, 460, 383},
    {750, 692, 643, 600, 500},
    {1000, 923, 857, 800, 667},
    {1100, 1015, 943, 880, 733},
    {1200, 1108, 1029, 960, 800},
    {1400, 1292, 1200, 1120, 933},
    {1600, 1477, 1371, 1280, 1067}};

int heightResidual[RESIDUAL_ROWS] = {5466, 4716, 3866, 3666, 3466, 2966, 2766};

// ======== NOVO: parâmetros de peso estático/dinâmico (formulaPeso) ========
float pBar = 0;               // bar
const float forkw_d = 375.0f; // tara dinâmica (garfos+sistema)
float offd = 0.0f;            // offset dinâmico

//============================= OFFSET =====================================================================
float NIVEL_BIESTAVEL = 0;    // offset primeiro estágio parado
float ELEVADO_2ESTAGIO = 0;   // offset segundo estágio parado
float MOVIMENTO1ESTAGIO = 50; // offset primeiro estágio movimentando
float MOVIMENTO2ESTAGIO = 0;  // offset segundo estágio movimentando
//===========================================================================================================

const float forkw_s = 104.0f;                                          // tara estática
const float offs = inSecondStage ? ELEVADO_2ESTAGIO : NIVEL_BIESTAVEL; // offset dinâmico

// velocidade do mastro (mm/s) – calculada por derivada de positionMm
static unsigned long _lastSpdTs = 0;
static float _lastPosMm = 0.0f;

// Amostragem de velocidade
#define SPEED_SAMPLE_INTERVAL 200 // ms

// Pulsos para velocidade (independente de encoderPosition)
volatile long encoderPulses = 0;
float mastSpeedMmPerSec = 0.0f;

// =========================== 2 - LED ===========================
// ------------------- PARÂMETROS DE CONTROLE DO LED STRIP -------------------
const int NUM_LEDS = 30;
const int DATA_PIN = 18;
const int WAVE_WIDTH = 3;
const int NUM_WAVES = 2;
int lastLedStripTime = 0;
const int LED_STRIP_FREQUENCY_CHECK = 50;
const float LED_SPEED_FACTOR = 10000;

unsigned long previousLedStripTime = 0;
int ledSpeed = 20;
uint8_t maxBrightness = 255;
uint8_t minBrightness = 0;

// ------------------- VARIÁVEIS LED -------------------
const CRGB COLOR_RED = CRGB::Red;
const CRGB COLOR_YELLOW = CRGB::Yellow;
const CRGB COLOR_GREEN = CRGB::Green;
const CRGB COLOR_VINHO = CRGB(128, 0, 64);
CRGB leds[NUM_LEDS];
CRGB ledStripColor = CRGB::Lime;

// ------------------- LEDS RELACIONADOS AO BLUETOOTH -------------------
bool ledAnimacaoNormalAtiva = false;
unsigned long tempoInicioPiscarAzul = 0;
int etapaPiscarAzul = 0;

// =========================== 3 - ALTURA / ENCODER / TORRE ===========================
// -------------------- CONTADOR DO ENCODER E DIREÇÃO -----------------------------
volatile long encoderPosition = 0;
volatile bool directionUp = false;

// -------------------- CÁLCULO DE POSIÇÃO E ALTURA-----------------------------
float positionMm = 0.0;
float startPositionMm;
const float FIRST_START_POSITION_MM = 0.0;
float SECOND_STAGE_START_HEIGHT_MM = 1819.0;

// -------------------- CONFIGURAÇÃO MECÂNICA DO ENCODER -----------------------------
const float ENCODER_FACTOR_RELATION = 2.0;                                // Fator de correção adicional aplicado à contagem do encoder para refletir a geometria real da torre ou sistema mecânico.
const int DRIVE_PULLEY_TEETH = 26;                                        // Quantidade de dentes da polia (pinhão) acoplada ao eixo que movimenta a torre da empilhadeira.
const float PROFILE_PITCH = 5.0;                                          // Passo do perfil T5 da correia (em mm).
const int PULSES_PER_REVOLUTION = 32;                                     // Quantidade de pulsos que o encoder incremental gera a cada rotação completa do eixo.
const float DISTANCE_PER_REVOLUTION = DRIVE_PULLEY_TEETH * PROFILE_PITCH; // Calcula a distância linear (em mm) que o garfo percorre a cada rotação completa da polia que aciona o encoder.
const float DISTANCE_PER_PULSE =
    (DISTANCE_PER_REVOLUTION / PULSES_PER_REVOLUTION) * ENCODER_FACTOR_RELATION;

const int DEBOUNCE_ENCODER_THRESHOLD = 200; // Valor em microssegundos usado como filtro de debounce para sinais do encoder.
const int MIN_ENC_COUNTER = 1;              // Define o número mínimo de pulsos necessários para considerar que houve movimento real.

// -------------------- LIMITES DE ALTURA E SEGURANÇA -----------------------------
float heightLimit = 3400.0;
int SECURITY_HEIGHT_LIMIT = 800;
int CRITICAL_HEIGHT_MARGIN = 500;
int HEIGHT_HYSTERESIS = 1000;

// -------------------- DETECÇÃO DE ESTÁGIOS DA TORRE -----------------------------
tipo_torre_t tipoTorre = TIPO_TORRE_TRIPLEX;
long encoderPositionAtSecondStage = 0;
bool secondStageStartRegistered = false;

// -------------------- VELOCIDADE E ACELERAÇÃO -----------------------------
const float LIMIAR_SPEED = 20.0; // Speed threshold in mm/s to consider as movement
// float speed = 0.0;
// float acceleration = 0.0; // Aceleração calculada
const float SPD_FACTOR_POS = -1.6;
const float SPD_FACTOR_NEG = -0.2;
const float ACCEL_FACTOR_POS = -0.5;
const float ACCEL_FACTOR_NEG = -0.25;

// =========================== 4 - RELÉS / SEGURANÇA ===========================
// -------------------- FLAGS DE SEGURANÇA E TRAVAS-----------------------------
bool shouldLockFromSafety = false;  // elevação
bool shouldLockFromTimeout = false; // tempo mais de 4 horas
bool bloqueioCargaAtivo = false;
bool bloqueioTemporarioAtivo = false;
bool travaSegurancaAtiva = false;

bool bloqueioInicial = false;

// -------------------- TEMPORIZADORES E CONTROLE DE TRAVA -----------------------------
unsigned long tempoInicioTravaSeguranca = 0;
const unsigned long TEMPO_MINIMO_TRAVADO_MS = 1000;

// -------------------- BUZZER (AVISO SONORO DE SEGURANÇA) -----------------------------
volatile int toggleBuzzer = 0;
volatile buzzer_state_t buzzerState = BUZZER_OFF;

// -------------------- FORK TIMEOUT (TEMPO COM GARFO ELEVADO) -----------------------------
float FORK_DOWN_THRESHOLD_MM = 50.0;
const unsigned long FORK_UP_TIMEOUT_MS = 14400000;
const unsigned long OPERATOR_RESPONSE_TIME_MS = 20000;

static unsigned long forkUpStartTime = 0; // Marca o momento em que o garfo foi elevado
static unsigned long buzzerStartTime = 0; // Marca o momento em que o buzzer começou a tocar
static bool forkTimeoutTriggered = false; // Indica se o tempo limite de elevação foi atingido
static bool driveBlocked = false;         // Indica se a tração foi bloqueada

// =========================== 5 - PWM / ELEVAÇÃO ===========================
// -------------------- ESTADO PWM DA ELEVAÇÃO  -----------------------------
bool pwmElevState = false;            // True quando PWM está ativo
unsigned long lastPWMStartTime = 0;   // Marca quando o PWM ficou HIGH
unsigned long pwmElevLastLowTime = 0; // Marca quando o PWM caiu para LOW

// -------------------- TEMPORIZAÇÃO / DETECÇÃO DE MOVIMENTO  -----------------------------
const int ANALOG_READ_FREQ_MOVING = 400; // Frequência de leitura em movimento (ms)
const int ANALOG_READ_FREQ_STATIC = 150; // Frequência de leitura parado (ms)
const int DELAY_MOVEMENT_START = 1000;   // Delay antes de considerar o movimento estabilizado

// -------------------- CONTROLE DE PARADA DA ELEVAÇÃO  -----------------------------
bool elevacaoParada = true; // True se a elevação está parada

// =========================== 6 - BLUETOOTH ===========================
unsigned long bleConnectionTime = 0;
bool notificationsEnabled = false;
bool setValue(uint8_t *value, size_t length);

bool bleLigado = false;
bool bleAutenticado = false;
unsigned long tempoInicialBLE = 0;

// -------------------- CONTRA - SENHA  -----------------------------
const char *AuthenticationKey = "Emp@2025!SLC"; // chave autenticação esperada
bool isAuthenticated = false;                   // controle de autenticação

// =========================== 7 - FUNÇÔES PESO (SENSOR, CÁLCULO, SUAVIZAÇÃO, MOVIMENTO) ===========================
void readAnalogInputs()
{
    digitalWrite(CS0_PIN, LOW);
    uint8_t highByte = SPI.transfer(0x00);
    uint8_t lowByte = SPI.transfer(0x00);
    digitalWrite(CS0_PIN, HIGH);
    uint16_t fullWord = ((highByte << 8) | lowByte) & 0x0FFF;
    uint32_t medida = fullWord * 1.221;
    adcBuffer[bufferIndex++] = medida;
    if (bufferIndex >= BUFFER_SIZE)
    {
        bufferIndex = 0;
        bufferFilled = true;
    }
    int samplesToAverage = bufferFilled ? BUFFER_SIZE : bufferIndex;
    uint32_t soma = 0;
    for (int i = 0; i < samplesToAverage; i++)
        soma += adcBuffer[i];
    weightSensorValue = soma / samplesToAverage;
}

float arredondarParaMultiploDe50(float peso)
{
    return round(peso / 50.0) * 50.0;
}

float interpolateResidualHeight(float pesoKg, int cc)
{

    // if (pesoEstabilizado <= 0.0 || NAN)
    // {
    //     return 5466;
    // }

    float pesoUsado = podeCongelarPeso ? pesoSuavizado : pesoEstabilizado;

    if (isnan(pesoUsado) || pesoUsado <= 0.0f)
    {
        return heightResidual[0]; // Altura máxima segura
    }

    int ccIdx = -1;

    // Localiza o índice do centro de carga exato
    for (int i = 0; i < RESIDUAL_COLS; i++)
    {
        if (ccValues[i] == cc)
        {
            ccIdx = i;
            break;
        }
    }

    if (ccIdx == -1)
        return 0; // CC não encontrado

    // Peso zero ou negativo retorna altura máxima
    if (pesoKg <= 0)
        return heightResidual[0];

    // Peso acima do máximo retorna altura mínima
    if (pesoKg >= weightResidual[RESIDUAL_ROWS - 1][ccIdx])
        return heightResidual[RESIDUAL_ROWS - 1];

    // Busca faixa e interpola
    for (int i = 1; i < RESIDUAL_ROWS; i++)
    {
        if (pesoKg <= weightResidual[i][ccIdx])
        {
            float pesoA = weightResidual[i - 1][ccIdx];
            float pesoB = weightResidual[i][ccIdx];
            float alturaA = heightResidual[i - 1];
            float alturaB = heightResidual[i];
            float fator = (pesoKg - pesoA) / (pesoB - pesoA);
            return alturaA + fator * (alturaB - alturaA);
        }
    }

    // fallback
    return heightResidual[RESIDUAL_ROWS - 1];
}

float interpolateResidualWeight(float alturaMm, int cc)
{
    int ccIdx = -1;

    for (int i = 0; i < RESIDUAL_COLS; i++)
    {
        if (ccValues[i] == cc)
        {
            ccIdx = i;
            break;
        }
    }

    if (ccIdx == -1)
        return 0;

    // Clamp altura
    if (alturaMm < heightResidual[RESIDUAL_ROWS - 1])
        alturaMm = heightResidual[RESIDUAL_ROWS - 1];
    else if (alturaMm > heightResidual[0])
        alturaMm = heightResidual[0];

    for (int i = 0; i < RESIDUAL_ROWS; i++)
    {
        if (alturaMm >= heightResidual[i]) // acha o primeiro menor ou igual
        {
            return weightResidual[i][ccIdx];
        }
    }

    // fallback de segurança
    return weightResidual[RESIDUAL_ROWS - 1][ccIdx];
}

void handlePWMState()
{
    bool rawPWM = digitalRead(PWM_ELEV);
    unsigned long currentTime = millis();
    if (rawPWM)
    {
        if (!pwmElevState)
        {
            pwmElevState = true;
            lastPWMStartTime = currentTime;
        }
    }
    else
    {
        if (pwmElevState && pwmElevLastLowTime == 0)
            pwmElevLastLowTime = currentTime;
        if (pwmElevLastLowTime > 0 && (currentTime - pwmElevLastLowTime >= 1000))
        {
            pwmElevState = false;
            pwmElevLastLowTime = 0;
        }
        if (pwmElevLastLowTime > 0 && rawPWM)
            pwmElevLastLowTime = 0;
    }
}

void processPesoEmMovimento()
{
    pesoMoviAtual = calculateWeightSpecial(weightSensorValue);
    if (pesoMoviAtual < 0)
        pesoMoviAtual = 0;
    if (!coletandoMovimento)
    {
        coletandoMovimento = true;
        indexMovimento = 0;
        pesoMoviCongelado = NAN;
        pesoMoviJaCongelado = false;
    }
    if (indexMovimento < 10)
        bufferMovimento[indexMovimento++] = pesoMoviAtual;
    if (indexMovimento >= 10 && !pesoMoviJaCongelado)
    {
        float soma = 0;
        for (int i = 0; i < 10; i++)
            soma += bufferMovimento[i];
        pesoMoviCongelado = soma / 10.0;
        pesoMoviJaCongelado = true;
    }
    // pesoEstabilizado = NAN;
    tempoEstabilizacao = 0;
    primeiroPeso = true;
}

float PESO_MINIMO_CONGELAR = 300.0;

// void processPesoEstabilizado()
// {
//     unsigned long currentTime = millis();

//     if (!isnan(pesoEstabilizado) && ultimoPesoValido < LIMIAR_ZERO)
//     {
//         pesoEstabilizado = NAN;
//         tempoEstabilizacao = 0;
//         primeiroPeso = true;
//         podeCongelarPeso = true;
//         return;
//     }

//     // 2. Só tenta congelar se ainda não tem peso estabilizado E a trava está liberada
//     if (isnan(pesoEstabilizado) && podeCongelarPeso)
//     {
//         if (ultimoPesoValido >= PESO_MINIMO_CONGELAR)
//         {
//             if (tempoEstabilizacao == 0)
//                 tempoEstabilizacao = currentTime;
//             else if (currentTime - tempoEstabilizacao >= TEMPO_HOLD)
//             {
//                 pesoEstabilizado = ultimoPesoValido;
//                 podeCongelarPeso = false; // Após congelar, trava até descarregar
//             }
//         }
//         else
//         {
//             // Se não tem carga, reseta o temporizador
//             tempoEstabilizacao = 0;
//         }
//     }
// }
// Parâmetros de segurança para congelamento
// const float PESO_MINIMO_CONGELAR = 300.0f;         // kg - evita congelar peso leve demais
const float TOLERANCIA_CONGELAMENTO = 80.0f; // kg - diferença aceitável entre suavizado e valor direto

void processPesoEstabilizado()
{
    unsigned long currentTime = millis();

    // Caso a carga tenha sido removida (peso abaixo do limiar) → limpa estado
    if (!isnan(pesoEstabilizado) && ultimoPesoValido < LIMIAR_ZERO)
    {
        pesoEstabilizado = NAN;
        tempoEstabilizacao = 0;
        primeiroPeso = true;
        podeCongelarPeso = true;
        ESP_LOGI("INFO", "Carga removida. Resetando peso estabilizado.");
        return;
    }

    // Só tenta congelar se ainda não há peso estabilizado e a trava está liberada
    if (isnan(pesoEstabilizado) && podeCongelarPeso && inSecondStage)

    {
        if (ultimoPesoValido >= PESO_MINIMO_CONGELAR)
        {
            // Primeira vez que detecta peso válido
            if (tempoEstabilizacao == 0)
            {
                tempoEstabilizacao = currentTime;
                ESP_LOGI("INFO", "Iniciando estabilização de peso. Valor lido: %.1f", ultimoPesoValido);
            }
            else if (currentTime - tempoEstabilizacao >= TEMPO_HOLD)
            {
                float diferenca = fabs(pesoSuavizado - ultimoPesoValido);

                if (diferenca <= TOLERANCIA_CONGELAMENTO)
                {
                    pesoEstabilizado = ultimoPesoValido;
                    podeCongelarPeso = false;
                    ESP_LOGI("OK", "Peso estabilizado com sucesso: %.1f", pesoEstabilizado);
                }
                else
                {
                    tempoEstabilizacao = 0; // reinicia tentativa
                    ESP_LOGW("AVISO", "Peso inconsistente com suavizado. Rejeitado.");
                    ESP_LOGI("AVISO", "Suavizado: %.1f | Lido: %.1f | Diferença: %.1f", pesoSuavizado, ultimoPesoValido, diferenca);
                }
            }
        }
        else
        {
            // Peso abaixo do mínimo permitido → não estabiliza
            tempoEstabilizacao = 0;
            ESP_LOGI("INFO", "Peso muito baixo para congelar: %.1f", ultimoPesoValido);
        }
    }
}

float lagrange4pts(double x, double *x_vals, double *y_vals)
{
    float result = 0.0;
    for (int i = 0; i < 4; i++)
    {
        float term = y_vals[i];
        for (int j = 0; j < 4; j++)
        {
            if (j != i)
                term *= (x - x_vals[j]) / (x_vals[i] - x_vals[j]);
        }
        result += term;
    }
    return result;
}

void findClosest4Points(double x, const double *x_vals, const double *y_vals, int n, double out_x[4], double out_y[4])
{
    int closest = 0;
    double minDiff = fabs(x_vals[0] - x);
    for (int i = 1; i < n; i++)
    {
        double diff = fabs(x_vals[i] - x);
        if (diff < minDiff)
        {
            minDiff = diff;
            closest = i;
        }
    }
    int idx0 = (closest <= 1) ? 0 : (closest >= n - 2) ? n - 4
                                                       : closest - 1;
    for (int i = 0; i < 4; i++)
    {
        out_x[i] = x_vals[idx0 + i];
        out_y[i] = y_vals[idx0 + i];
    }
}

float corrigirErroSensor(float sensor)
{
    double erro_mV[] = {975, 1010, 1190};
    double erro_real[] = {-5, 18, 2};
    int n = sizeof(erro_mV) / sizeof(double);
    for (int i = 0; i < n - 1; i++)
    {
        if (sensor >= erro_mV[i] && sensor <= erro_mV[i + 1])
        {
            double x0 = erro_mV[i], x1 = erro_mV[i + 1];
            double y0 = erro_real[i], y1 = erro_real[i + 1];
            return y0 + ((sensor - x0) * (y1 - y0)) / (x1 - x0);
        }
    }
    return 0.0;
}

float calculateWeightSpecial(float sensorValue)
{
    float result = 0.0;
    for (int i = 0; i < ARRAY_SIZE_SPECIAL; i++)
    {
        float term = WT_SPECIAL[i];
        for (int j = 0; j < ARRAY_SIZE_SPECIAL; j++)
        {
            if (j != i)
                term *= (sensorValue - V_SPECIAL[j]) / (V_SPECIAL[i] - V_SPECIAL[j]);
        }
        result += term;
    }
    return result + corrigirErroSensor(sensorValue);
}

float calculateWeightSpecialStage2(float sensorValue)
{
    float result = 0.0;
    for (int i = 0; i < ARRAY_SIZE_SPECIAL_STAGE2; i++)
    {
        float term = WT_SPECIAL_STAGE2[i];
        for (int j = 0; j < ARRAY_SIZE_SPECIAL_STAGE2; j++)
        {
            if (j != i)
                term *= (sensorValue - V_SPECIAL_STAGE2[j]) / (V_SPECIAL_STAGE2[i] - V_SPECIAL_STAGE2[j]);
        }
        result += term;
    }
    return result + corrigirErroSensor(sensorValue);
}

float calculateWeightFinal()
{
    double temp_x[4], temp_y[4];
    const double *x_vals, *y_vals;
    int n;

    if (inSecondStage && pwmElevState)
        return calculateWeightSpecialStage2(weightSensorValue);
    else if (!inSecondStage && pwmElevState)
        return calculateWeightSpecial(weightSensorValue);

    if (inSecondStage)
    {
        x_vals = V2_AD;
        y_vals = WT_STAGE2;
        n = ARRAY_SIZE_STAGE2;
    }
    else
    {
        x_vals = V1_AD;
        y_vals = WT_STAGE1;
        n = ARRAY_SIZE_STAGE1;
    }
    if (weightSensorValue < x_vals[0])
        return 0.0;
    if (weightSensorValue > x_vals[n - 1])
        return NAN;

    findClosest4Points(weightSensorValue, x_vals, y_vals, n, temp_x, temp_y);
    return lagrange4pts(weightSensorValue, temp_x, temp_y) + corrigirErroSensor(weightSensorValue);
}

void detectarCargaInstantanea()
{
    unsigned long agora = millis();

    // Detecta nova carga
    if (!cargaDetectada && pesoAnterior < LIMIAR_CARGA && pesoSuavizado > LIMIAR_CARGA)
    {
        bloqueioCargaAtivo = true;
        tempoInicioBloqueio = agora;
        cargaDetectada = true;
        pesoJaCongelado = false; // permite congelamento após delay
    }

    // Após 2s: libera elevação e congela peso se ainda não congelado
    if (bloqueioCargaAtivo && (agora - tempoInicioBloqueio >= TEMPO_BLOQUEIO_MS))
    {
        bloqueioCargaAtivo = false;

        if (!pesoJaCongelado)
        {
            pesoCongeladoPorCarga = pesoSuavizado;
            pesoJaCongelado = true;
        }
    }

    // Reset se carga foi removida
    if (pesoSuavizado < LIMIAR_CARGA)
    {
        cargaDetectada = false;
        pesoJaCongelado = false;
    }

    pesoAnterior = pesoSuavizado;
}
static float pesoAnteriorCongelamento = 0.0;
void congelarPesoPorCargaComDelay()
{
    static bool cargaDetectada = false;
    static unsigned long tempoInicioDeteccao = 0;
    const float LIMIAR_CARGA_CONG = 1000.0;
    const unsigned long TEMPO_DELAY_CONGELAMENTO = 2000;

    unsigned long agora = millis();

    // Detecta início de nova carga
    if (!cargaDetectada && pesoAnteriorCongelamento < LIMIAR_CARGA_CONG && pesoSuavizado > LIMIAR_CARGA_CONG)
    {
        tempoInicioDeteccao = agora;
        cargaDetectada = true;
    }

    // Após 2s com carga presente, congela o peso
    if (cargaDetectada && (agora - tempoInicioDeteccao >= TEMPO_DELAY_CONGELAMENTO))
    {
        pesoCongeladoPorCarga = pesoSuavizado;
        cargaDetectada = false; // Evita múltiplos congelamentos
    }

    // Reseta detecção se a carga foi removida antes do tempo
    if (pesoSuavizado < LIMIAR_CARGA_CONG)
    {
        cargaDetectada = false;
    }

    pesoAnteriorCongelamento = pesoSuavizado;
}

// Converte milivolts do ADC para bar (0.5–4.5V => 0–300 bar)
float convertMvToBar(float mv)
{
    return ((mv - 500.0f) / 4000.0f) * 300.0f;
}

// Ganho estático dependente de estágio/pressão
float getKspByPressure(float p)
{
    if (inSecondStage)
    {
        if (p < 59.9f)
            return 10.908f; // Faixa leve
        else if (p < 132.75f)
            return 10.225f; // Faixa média
        else
            return 10.126f; // Faixa pesada
    }
    else
    {
        if (p < 60.0f)
            return 11.6864f; // Faixa leve (bar médio ≈ 31.5)
        else if (p < 110.0f)
            return 12.0610f; // Faixa média (bar médio ≈ 90.0)
        else
            return 12.0398f; // Faixa pesada (bar médio ≈ 130.07)
    }
}

// Peso estático (usa ksp(p))
float calculateStaticWeight(float mv)
{
    float p = convertMvToBar(mv);
    float ksp = getKspByPressure(p);
    float forkw = inSecondStage ? forkw_d : forkw_s;
    float ws = ksp * p - forkw + offs;
    return (ws < 0.0f) ? 0.0f : ws;
}

// Peso dinâmico (pressão + velocidade)
float calculateDynamicWeight(float pBar, float velMmPs)

{
    float kdp = inSecondStage ? 9.7225f : 11.5625f; // ganho pressão dinâmico
    float kdv = inSecondStage ? 0.06f : -0.44f;     // ganho velocidade dinâmico

    float forkw = inSecondStage ? forkw_d : forkw_s;
    offd = inSecondStage ? MOVIMENTO2ESTAGIO : MOVIMENTO1ESTAGIO;
    float wd = kdp * pBar + kdv * velMmPs - forkw + offd;
    return (wd < 0.0f) ? 0.0f : wd;
}

// Velocidade do mastro baseada nos pulsos
/* float calcularVelocidade()
{
    static unsigned long lastSpeedCheck = 0;
    unsigned long now = millis();
    float velocidade = mastSpeedMmPerSec;

    if (now - lastSpeedCheck >= SPEED_SAMPLE_INTERVAL)
    {
        noInterrupts();
        long pulses = encoderPulses;
        encoderPulses = 0;
        interrupts();

        float dt = SPEED_SAMPLE_INTERVAL / 1000.0f;
        velocidade = (pulses * DISTANCE_PER_PULSE) / dt; // mm/s
        mastSpeedMmPerSec = velocidade;
        lastSpeedCheck = now;
    }
    return velocidade;
}
 */
// Considerando velocidade negativa
float calcularVelocidade()
{
    static unsigned long lastSpeedCheckTime = 0;
    // Store the last known encoder position for calculating the difference.
    static long lastEncoderPosForSpeed = 0;
    unsigned long now = millis();

    // Check at regular intervals
    if (now - lastSpeedCheckTime >= SPEED_SAMPLE_INTERVAL)
    {
        long currentEncoderPos = encoderPosition;

        // Calculate the difference in position (in pulses) since the last check
        long positionChangeInPulses = currentEncoderPos - lastEncoderPosForSpeed;

        // Calculate the time elapsed in seconds
        float deltaTime = (now - lastSpeedCheckTime) / 1000.0f;

        // Calculate speed (mm/s). This will be positive or negative.
        if (deltaTime > 0)
        {
            mastSpeedMmPerSec = (positionChangeInPulses * DISTANCE_PER_PULSE) / deltaTime;
        }

        // Update the state for the next calculation
        lastSpeedCheckTime = now;
        lastEncoderPosForSpeed = currentEncoderPos;
    }

    // Return the most recently calculated speed
    return mastSpeedMmPerSec;
}

// =========================== 8 - LED (INDICADORES VISUAIS E ANIMAÇÕES) ===========================

void wavePattern(CRGB color, int speed, bool direction, bool enableWaveEffect)
{
    static int positions[NUM_WAVES];
    static bool initialized = false;
    static unsigned long lastUpdate = 0;

    if (!initialized)
    {
        for (int i = 0; i < NUM_WAVES; i++)
        {
            positions[i] = i * (NUM_LEDS / NUM_WAVES);
        }
        initialized = true;
    }

    if (!enableWaveEffect)
    {
        fill_solid(leds, NUM_LEDS, color);
        FastLED.show();
        return;
    }

    // Controle de tempo baseado na variável speed
    unsigned long now = millis();
    if (now - lastUpdate < speed)
        return;
    lastUpdate = now;

    fill_solid(leds, NUM_LEDS, CRGB::Black);

    for (int w = 0; w < NUM_WAVES; w++)
    {
        for (int i = -WAVE_WIDTH / 2; i <= WAVE_WIDTH / 2; i++)
        {
            int ledPos = positions[w] + i;
            if (ledPos >= NUM_LEDS)
                ledPos -= NUM_LEDS;
            if (ledPos < 0)
                ledPos += NUM_LEDS;

            uint8_t brightness = maxBrightness - abs(i) * ((maxBrightness - minBrightness) / (WAVE_WIDTH / 2 + 1));
            brightness = max(brightness, minBrightness);

            leds[ledPos] = color;
            leds[ledPos].fadeLightBy(256 - brightness);
        }

        // Atualiza a posição do pico da onda
        if (direction)
            positions[w] = (positions[w] + 1) % NUM_LEDS;
        else
            positions[w] = (positions[w] - 1 + NUM_LEDS) % NUM_LEDS;
    }

    FastLED.show();
}

void updateLedVisual(float pesoAtual)
{
    // 08-09
    bool acimaDoBiestavel = (digitalRead(BIESTAVEL) == HIGH);

    unsigned long currentTime = millis();

    if (currentTime - lastLedStripTime > LED_STRIP_FREQUENCY_CHECK)
    {
        lastLedStripTime = currentTime;

        CRGB selectedColor = COLOR_GREEN;
        bool enableWave = true;

        // ───────────────────────────────────────────────
        // CENÁRIO 1: ALTURA MÁXIMA OU PESO ACIMA DO LIMITE → PISCAR VERMELHO
        if (!inSecondStage && acimaDoBiestavel)
        {
            const float LIMITE_PESO_CRITICO = 1650.0f;

            float pesoLimite = interpolateResidualWeight(positionMm, 600);
            if (pesoAtual > LIMITE_PESO_CRITICO)
            {

                static bool blinkState = false;
                if (currentTime - previousLedStripTime > 400)
                {
                    previousLedStripTime = currentTime;
                    blinkState = !blinkState;
                    fill_solid(leds, NUM_LEDS, blinkState ? COLOR_RED : CRGB::Black);
                    FastLED.show();
                }
                return;
            }
        }

        // 08-09
        if (!inSecondStage && !acimaDoBiestavel && pesoSuavizado > 200.0f)
        {
            static bool blinkState = false;
            if (currentTime - previousLedStripTime > 400)
            {
                previousLedStripTime = currentTime;
                blinkState = !blinkState;
                fill_solid(leds, NUM_LEDS, blinkState ? COLOR_GREEN : CRGB::Black);
                FastLED.show();
            }
            return;
        }
        if (inSecondStage && positionMm >= heightLimit)
        {
            static bool blinkState = false;
            if (currentTime - previousLedStripTime > 400)
            {
                previousLedStripTime = currentTime;
                blinkState = !blinkState;
                fill_solid(leds, NUM_LEDS, blinkState ? COLOR_RED : CRGB::Black);
                FastLED.show();
            }
            return;
        }

        // ───────────────────────────────────────────────
        // CENÁRIO 2: MUITO PRÓXIMO DO LIMITE → VERMELHO
        else if (positionMm >= (heightLimit - CRITICAL_HEIGHT_MARGIN))
        {
            selectedColor = COLOR_RED;
        }

        // CENÁRIO 3: ZONA INTERMEDIÁRIA → AMARELO
        else if (positionMm >= (heightLimit - SECURITY_HEIGHT_LIMIT))
        {
            selectedColor = COLOR_YELLOW;
        }

        // CENÁRIO 4: ZONA SEGURA → VERDE
        else if (positionMm < (heightLimit - HEIGHT_HYSTERESIS))
        {
            selectedColor = COLOR_GREEN;
        }

        // CENÁRIO 5: EMPILHADEIRA PARADA → COR FIXA DO CENÁRIO ATUAL
        if (elevacaoParada)
        {
            if (currentTime - previousLedStripTime > 300)
            {
                previousLedStripTime = currentTime;
                fill_solid(leds, NUM_LEDS, selectedColor); // mantém cor do cenário ativo
                FastLED.show();
            }
        }

        // MOVIMENTO DETECTADO → ATIVA EFEITO DE ONDA COM A COR DO CENÁRIO
        else
        {
            if (currentTime - previousLedStripTime > 100)
            {
                previousLedStripTime = currentTime;
                wavePattern(selectedColor, velocidadeParaCalculo, directionUp, true);
            }
        }
    }
}

void piscarLedVinho()
{
    static unsigned long ultimaTroca = 0;
    static bool estado = false;
    unsigned long agora = millis();

    // Pisca a cada 300ms
    if (agora - ultimaTroca > 300)
    {
        ultimaTroca = agora;
        estado = !estado;
        fill_solid(leds, NUM_LEDS, estado ? COLOR_VINHO : CRGB::Black);
        FastLED.show();
    }
}

void BluetoothLedAzul()
{
    static bool estado = false;
    static unsigned long ultimoToggle = 0;

    if (etapaPiscarAzul >= 6)
    {
        // Terminou 3 piscadas (6 etapas: ON/OFF alternados)
        ledAnimacaoNormalAtiva = true;
        etapaPiscarAzul = 0;
        return;
    }

    unsigned long agora = millis();
    if (agora - ultimoToggle >= 500)
    {
        ultimoToggle = agora;
        estado = !estado;

        fill_solid(leds, NUM_LEDS, estado ? CRGB::Blue : CRGB::Black);
        FastLED.show();

        etapaPiscarAzul++;
    }
}

// =========================== 9 - ALTURA / ENCODER / TORRE ===========================
void checkLiftStage()
{
    static bool ultimoEstadoSensor = false;
    static bool transicaoValidada = false;

    // CENÁRIO 1: Torre do tipo DUPLEX → nunca entra em segundo estágio
    if (tipoTorre == TIPO_TORRE_DUPLEX)
    {
        inSecondStage = false;
        secondStageStartRegistered = false;
        transicaoValidada = false;
        return;
    }

    bool sensorNA = digitalRead(IND_NA);
    bool sensorNF = digitalRead(IND_NF);
    bool sensorAtivo = (sensorNA == HIGH && sensorNF == LOW);

    // CENÁRIO 2: Sensor foi recém-ativado → reinicia validação
    if (sensorAtivo && !ultimoEstadoSensor)
    {
        transicaoValidada = false;
    }

    // CENÁRIO 3: Sensor ativo, torre subindo, encoder contou → confirma segundo estágio
    if (sensorAtivo && directionUp && !transicaoValidada)
    {
        if (encoderPosition > MIN_ENC_COUNTER)
        {
            secondStageStartRegistered = true;
            inSecondStage = true;
            transicaoValidada = true;
        }
    }

    // CENÁRIO 4: Sensor desligado ou torre descendo sem validar → reset
    if (!sensorAtivo || (!directionUp && !transicaoValidada))
    {
        secondStageStartRegistered = false;
        inSecondStage = false;
        transicaoValidada = false;
    }

    ultimoEstadoSensor = sensorAtivo;
}

void encoderA()
{
    if (digitalRead(ENC_A) == !digitalRead(ENC_B))
    {
        encoderPosition++;
        directionUp = true;
        encoderPulses++;
    }
}

void encoderB()
{
    if (digitalRead(ENC_B) == !digitalRead(ENC_A))
    {
        if (encoderPosition > 0)
        {
            encoderPosition--;
        }
        else
        {
            encoderPosition = 0;
        }
        directionUp = false;
    }
}

void IRAM_ATTR encoderISR()
{
    encoderPulses++;
}

void updatePositionMm()
{
    static bool estavaNoSegundoEstagio = false;

    if (tipoTorre == TIPO_TORRE_TRIPLEX)
    {
        if (inSecondStage)
        {
            positionMm = SECOND_STAGE_START_HEIGHT_MM + (encoderPosition * DISTANCE_PER_PULSE);
            estavaNoSegundoEstagio = true;
        }
        else
        {
            if (estavaNoSegundoEstagio)
            {
                encoderPosition = 0;
                estavaNoSegundoEstagio = false;
            }

            positionMm = encoderPosition * DISTANCE_PER_PULSE;
        }
    }
    else
    {
        positionMm = encoderPosition * DISTANCE_PER_PULSE;
    }
}

void updateElevacaoParada()
{
    static long lastEncoderPosition = 0;
    static unsigned long lastCheckTime = 0;
    static bool wasMoving = false; // Detecta transição de movimento → parada

    unsigned long now = millis();
    if (now - lastCheckTime >= 200)
    { // checa a cada 200ms
        if (encoderPosition != lastEncoderPosition)
        {
            elevacaoParada = false;
            lastEncoderPosition = encoderPosition;
            wasMoving = true;
        }
        else
        {
            elevacaoParada = true;

            // Se estava se movendo antes e agora parou → salvar valor
            if (wasMoving)
            {
                Preferences preferences;
                preferences.begin("SLC", false); // modo escrita
                preferences.putLong("enc_pos", encoderPosition);
                preferences.end();
                wasMoving = false;
            }
        }
        lastCheckTime = now;
    }
}

// 08-09
const float PESO_MARGEM_BIESTAVEL = 1800.0f;
// =========================== 10 - SEGURANÇA / RELÉS / TIMEOUT ===========================
void applySafetyRestrictions(float pesoAtual)
{
    shouldLockFromSafety = false;
    bool acimaDoBiestavel = (digitalRead(BIESTAVEL) == HIGH);

    // CENÁRIO 1: Limite ou peso excedeu o permitido
    // Ação: Buzzer contínuo e elevação travada via RELAY2

    if (!inSecondStage && !acimaDoBiestavel && pesoAtual > PESO_MARGEM_BIESTAVEL)
    {
        buzzerState = BUZZER_BEEP_CONTINUOUS;
        shouldLockFromSafety = true;
        return;
    }

    if (pesoAtual > 1650)
    {
        buzzerState = BUZZER_BEEP_CONTINUOUS;
        shouldLockFromSafety = true;
        return;
    }
    else
    {
        buzzerState = BUZZER_OFF;
        shouldLockFromSafety = false;
    }

    if (inSecondStage && positionMm >= heightLimit)

    {
        buzzerState = BUZZER_BEEP_CONTINUOUS;
        shouldLockFromSafety = true;
    }

    // // CENÁRIO 2: Muito próximo do limite final (zona crítica)
    // // Ação: Buzzer contínuo (sem travar ainda)
    else if (positionMm >= (heightLimit - CRITICAL_HEIGHT_MARGIN))
    {
        buzzerState = BUZZER_BEEP_SHORT;
    }

    // // CENÁRIO 3: Faixa intermediária de segurança (zona de alerta)
    // // Ação: Buzzer curto intermitente para alertar operador
    else if (positionMm >= (heightLimit - SECURITY_HEIGHT_LIMIT) && positionMm < heightLimit)
    {
        buzzerState = BUZZER_BEEP_SHORT;
    }

    // // CENÁRIO 4: Zona segura (altura abaixo da histerese)
    // // Ação: Buzzer desligado, elevação liberada
    else if (positionMm < (heightLimit - HEIGHT_HYSTERESIS))
    {
        buzzerState = BUZZER_OFF;
        shouldLockFromSafety = false; // Libera elevação
    }

    // CENÁRIO 5: Fora das zonas definidas (ex: parado ou entre faixas)
    // Ação: Buzzer desligado, sem mexer no relé
    else
    {
        buzzerState = BUZZER_OFF;
    }
}

// Variáveis mockadas que deverão ser parametrizáveis futuramente

void handleForkLogic()
{

    bool forkElevated = (positionMm > FORK_DOWN_THRESHOLD_MM);
    shouldLockFromTimeout = false;

    if (forkElevated)
    {
        // CENÁRIO 1: Garfo acaba de ser elevado → inicia contagem do tempo de elevação
        if (forkUpStartTime == 0)
        {
            forkUpStartTime = millis();
        }

        // CENÁRIO 2: Garfo permaneceu elevado por mais de 4h → dispara alarme sonoro
        if (!forkTimeoutTriggered && (millis() - forkUpStartTime >= FORK_UP_TIMEOUT_MS))
        {
            buzzerState = BUZZER_BEEP_CONTINUOUS;
            buzzerStartTime = millis();
            forkTimeoutTriggered = true;
        }

        // CENÁRIO 3: Passaram-se 20 segundos após o alarme e garfo ainda está elevado → bloqueia tração
        if (forkTimeoutTriggered && (millis() - buzzerStartTime >= OPERATOR_RESPONSE_TIME_MS))
        {
            if (positionMm > FORK_DOWN_THRESHOLD_MM)
            {
                driveBlocked = true;
                shouldLockFromTimeout = true;
            }
        }
    }
    else
    {
        // CENÁRIO 4: Garfo foi abaixado → reinicia todo o controle (sem alarme nem bloqueio)
        forkUpStartTime = 0;
        forkTimeoutTriggered = false;
        driveBlocked = false;
    }

    // Arrumar para o RELAY PIN correto da tração
    if (driveBlocked)
    {
        // digitalWrite(RELAY1_PIN, HIGH);
    }
    else
    {
        digitalWrite(RELAY1_PIN, LOW);
    }
}

bool travarAgora;

void applyFinalRelayState()
{
    travarAgora = shouldLockFromSafety || shouldLockFromTimeout || bloqueioCargaAtivo || bloqueioInicial;
    unsigned long agora = millis();

    if (travarAgora)
    {
        travaSegurancaAtiva = true;
        tempoInicioTravaSeguranca = agora;
        digitalWrite(RELAY2_PIN, HIGH);
        digitalWrite(RELAY1_PIN, HIGH);
    }
    else if (travaSegurancaAtiva)
    {
        if ((agora - tempoInicioTravaSeguranca) >= TEMPO_MINIMO_TRAVADO_MS)
        {
            travaSegurancaAtiva = false;
            digitalWrite(RELAY2_PIN, LOW);
            digitalWrite(RELAY1_PIN, LOW);
        }
        else
        {
            digitalWrite(RELAY2_PIN, HIGH);
            digitalWrite(RELAY1_PIN, HIGH);
        }
    }
    else
    {
        digitalWrite(RELAY2_PIN, LOW);
        digitalWrite(RELAY1_PIN, LOW);
    }
}

void buzzerControl()
{
    static unsigned long lastToggleTime = 0;
    static bool buzzerOn = false;
    static int beepCount = 0;
    const unsigned long BEEP_SHORT_TOGGLE_INTERVAL = 200; // 200ms on/off
    const int MAX_BEEP_CYCLES = 4;                        // ligado/desligado * 2 (ex: 2 beeps = 4 ciclos)

    unsigned long now = millis();

    switch (buzzerState)
    {
    case BUZZER_OFF:
        digitalWrite(BUZZER_PIN, LOW);
        buzzerOn = false;
        beepCount = 0;
        break;

    case BUZZER_BEEP_SHORT:
        if (now - lastToggleTime >= BEEP_SHORT_TOGGLE_INTERVAL)
        {
            buzzerOn = !buzzerOn;
            digitalWrite(BUZZER_PIN, buzzerOn ? HIGH : LOW);
            lastToggleTime = now;
            beepCount++;

            if (beepCount >= MAX_BEEP_CYCLES)
            {
                buzzerState = BUZZER_OFF;
            }
        }
        break;

    case BUZZER_BEEP_CONTINUOUS:
        digitalWrite(BUZZER_PIN, LOW);
        buzzerOn = true;
        break;
    }
}

// =========================== 11 - SISTEMA / ESTADO / DIAGNÓSTICO ===========================
void exibirStatus(float pesoAtual, float duty_cycle, float estimated_speed)
{
    ESP_LOGI("STATUS", "\n====== STATUS ======");
    ESP_LOGI("STATUS", "PWM_ELEV: %s", pwmElevState ? "ELEVANDO" : "PARADO");
    ESP_LOGI("STATUS", "Sensor (mV): %.1f", weightSensorValue);
    ESP_LOGI("STATUS", "Pressão   : %.1f", pBar);
    ESP_LOGI("STATUS", "Peso considerado   : %.1f", pesoSeguro);
    ESP_LOGI("STATUS", "Peso atual       : %.1f", pesoAtual);
    ESP_LOGI("STATUS", "Peso suavizado   : %.1f", pesoSuavizado);
    ESP_LOGI("STATUS", "Peso congelado   : %.1f", isnan(pesoEstabilizado) ? 0 : pesoEstabilizado);
    ESP_LOGI("STATUS", "pesoAnteriorCongelamento   : %.1f", pesoAnteriorCongelamento);
    ESP_LOGI("STATUS", "pesoCongelado2Segundos  : %.1f", pesoCongelado2Segundos);
    ESP_LOGI("STATUS", "Altura: %d", (int)positionMm);
    ESP_LOGI("STATUS", "Altura máxima: %d", (int)heightLimit);
    ESP_LOGI("STATUS", "ENCODER: %ld", encoderPosition);
    ESP_LOGI("STATUS", "Velocidade   : %.1f", mastSpeedMmPerSec);
    // 3. Print the results.
    ESP_LOGI("STATUS", "Measured Duty Cycle: %.2f %%  |  Estimated Speed: %.2f mm/s", duty_cycle, estimated_speed);
    bool biestavelState = digitalRead(BIESTAVEL);
    ESP_LOGI("STATUS", "BIESTAVEL: %s", biestavelState == LOW ? "FECHADO" : "ABERTO");
}

void printQuandoCarregar()
{
    static bool tinhaCarga = false;       // guarda estado anterior
    const float LIMIAR_CARGA_PRINT = 5.0; // kg, evita disparo com ruído

    bool temCargaAgora = (pesoSuavizado > LIMIAR_CARGA_PRINT);

    // Detecta subida: antes não tinha, agora tem
    if (!tinhaCarga && temCargaAgora)
    {
        ESP_LOGI("INFO", "Carga detectada nos garfos!");
    }

    tinhaCarga = temCargaAgora;
}

// =========================== 12 -  BLUETOOTH (BLE E COMANDOS DO APP) ===========================
BLECharacteristic *pCharacteristicTX; // Autenticação
BLEAdvertising *pAdvertising;
bool deviceConnected = false;
unsigned long lastBLESend = 0;                // Controle de tempo para envio de mensagens BLE com intervalo
const unsigned long BLE_SEND_INTERVAL = 1000; // Intervalo mínimo entre envios BLE, em milissegundos
uint8_t bleStep = 0;                          // controle de estado

// UUIDs do serviço e characteristics (comunicação BLE customizada padrão Nordic UART)
#define SERVICE_UUID "6E400001-B5A3-F393-E0A9-E50E24DCCA9E"
#define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E" // TX: Dispositivo → App
#define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E" // RX: App → Dispositivo

// ENVIA DADOS EM PARTES VIA BLUETOTTH
void sendBLE(BLECharacteristic *characteristic, const String &message)
{
    const size_t CHUNK_SIZE = 180; // seguro para BLE com MTU negociado
    size_t len = message.length();

    // Envia a mensagem em pedaços de no máximo 180 bytes
    for (size_t i = 0; i < len; i += CHUNK_SIZE)
    {
        String chunk = message.substring(i, i + CHUNK_SIZE);
        characteristic->setValue((uint8_t *)chunk.c_str(), chunk.length());
        characteristic->notify();
    }
}

// Função auxiliar para logar mensagens no Serial e enviar via BLE
void logMessage(String msg)
{
    // Loga a mensagem usando o ESP-IDF Logging
    ESP_LOGI("LOG", "%s", msg.c_str());

    // Garante que o sendBLE receba com \n no final para não colar com próxima leitura
    if (!msg.endsWith("\n"))
    {
        msg += "\n";
    }

    sendBLE(pCharacteristicTX, msg);
}

// Classe Callback para tratar escrita na characteristic RX (comandos do celular)
class MyCallBacks : public BLECharacteristicCallbacks
{
    void onWrite(BLECharacteristic *pCharacteristic) override
    {
        // Obtém os dados recebidos como string
        String allData = String(pCharacteristic->getValue().c_str());

        allData.trim();

        while (allData.length() > 0)
        {
            int newlineIndex = allData.indexOf('\n');
            String line;

            if (newlineIndex >= 0)
            {
                // Se encontrar \n, extrai a primeira linha
                line = allData.substring(0, newlineIndex);
                allData = allData.substring(newlineIndex + 1);
            }
            else
            {
                // Se não houver mais quebras, pega tudo
                line = allData;
                allData = "";
            }

            line.trim(); // Limpa espaços

            // --- AUTENTICAÇÃO ---
            if (line.startsWith("AUTH:"))
            {
                // Extrai a chave recebida
                line = line.substring(5); // Remove "AUTH:"
                line.trim();
                String receivedKey = line;

                // Verifica se a chave recebida é válida
                if (receivedKey == AuthenticationKey)
                {
                    isAuthenticated = true;
                    bleAutenticado = true; // impede desligamento bluetooth

                    ESP_LOGI("AUTH", "Cliente autenticado com sucesso.");
                    sendBLE(pCharacteristicTX, "ACK:AUTENTICADO\n"); // Confirma para o app
                }
                else
                {
                    isAuthenticated = false;
                    ESP_LOGW("AUTH", "Falha na autenticação.");
                    sendBLE(pCharacteristicTX, "ACK:FALHA\n"); // Informa falha
                }
                return;
            }

            // Se não autenticado, ignora qualquer outro comando
            if (!isAuthenticated)
            {
                ESP_LOGW("AUTH", "Comando ignorado. Cliente não autenticado.");
                return;
            }

            if (line.length() > 0)
            {
                // Protege contra comandos colados como: SET:H:...Encoder:...
                int idxEncoder = line.indexOf("Encoder:");
                if (idxEncoder > 0)
                {
                    line = line.substring(0, idxEncoder);
                    line.trim();
                }

                processarComando(line);
            }
        }
    }
};

// Callback do servidor BLE (quando cliente conecta ou desconecta)
class MyServerCallbacks : public BLEServerCallbacks
{
public:
    void onConnect(BLEServer *pServer) override
    {
        deviceConnected = true;
        bleConnectionTime = millis(); // Marca o tempo de conexão
        bleStep = 0;
        etapaPiscarAzul = 0;
        ledAnimacaoNormalAtiva = false;

        ESP_LOGI("BLE", "Cliente conectado");
    }

    void onDisconnect(BLEServer *pServer) override
    {
        deviceConnected = false;
        isAuthenticated = false;

        // Volta LED para estado normal (verde)
        fill_solid(leds, NUM_LEDS, CRGB::Green);
        FastLED.show();

        // Desbloqueia a maquina
        digitalWrite(RELAY1_PIN, LOW);
        ESP_LOGI("BLE", "Cliente Desconectado");

        if (bleAutenticado)
        {
            ESP_LOGI("BLE", "BLE desligado após desconexão do cliente autenticado");
            logMessage("INFO:BLE desligado após desconexão do cliente autenticado");
            BLEDevice::deinit(true);
            bleLigado = false;
        }
        else
        {
            // Recomeça o advertising BLE
            pAdvertising->start();
        }
    }
};

// COR LED INDICATIVA BLUETOOTH
/*fill_solid(leds, NUM_LEDS, CRGB::Blue);
   FastLED.show();

   //BLOQUEIO DE MAQUINA(Rele tração)
   //digitalWrite(RELAY1_PIN, HIGH);
   }*/

// Inicializa o servidor BLE, serviços e características
void bluetoothConnection()
{
    BLEServer *pServer = BLEDevice::createServer();
    pServer->setCallbacks(new MyServerCallbacks());

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

    // Characteristic RX (escrita vinda do celular)
    BLECharacteristic *pCharacteristicRX = pService->createCharacteristic(
        CHARACTERISTIC_UUID_RX, BLECharacteristic::PROPERTY_WRITE | BLECharacteristic::PROPERTY_WRITE_NR);
    pCharacteristicRX->setCallbacks(new MyCallBacks());
    pCharacteristicRX->addDescriptor(new BLE2902());

    // Characteristic TX (notificações para o app)
    pCharacteristicTX = pService->createCharacteristic(
        CHARACTERISTIC_UUID_TX, BLECharacteristic::PROPERTY_NOTIFY);
    pCharacteristicTX->addDescriptor(new BLE2902()); // add BLE2902 normalmente

    pService->start();

    // Inicia advertising
    pAdvertising = BLEDevice::getAdvertising();
    pAdvertising->addServiceUUID(SERVICE_UUID);
    pAdvertising->start();
}

// Função chamada periodicamente para enviar dados de sensores via BLE
void bluetoothReceiveData()
{
    if (deviceConnected)
    {
        if (!isAuthenticated)
        {
            ESP_LOGW("BLE", "Ignorando mensagem. Dispositivo ainda não autenticado.");
            return;
        }

        unsigned long now = millis();
        if (now - lastBLESend >= BLE_SEND_INTERVAL)
        {
            lastBLESend = now;

            char sensorMsg[256];
            snprintf(sensorMsg, sizeof(sensorMsg),
                     "Encoder: %ld, PWM_ELEV:%s, Direction:%s, Sensor (mV): %d, Peso suavizado: %d, Peso movimentando: %d, Altura: %d, Altura máxima: %d, interpolar %d, Peso congelado: %d",
                     encoderPosition,
                     pwmElevState ? "ATIVO" : "INATIVO",
                     directionUp ? "UP" : "DOWN",
                     (int)weightSensorValue,
                     (int)pesoSuavizado,
                     (int)pesoMoviAtual,
                     (int)positionMm,
                     (int)heightLimit,
                     (int)interpolateResidualWeight(positionMm, 600),
                     (int)pesoEstabilizado);

            sendBLE(pCharacteristicTX, String(sensorMsg) + "\n");
            ESP_LOGI("BLE_DATA", "%s", sensorMsg);
        }
    }
}

void atualizarReles(float pesoAtual)
{
    if (!isnan(pesoAtual) && pesoAtual < 700.0)
    {
        digitalWrite(RELAY1_PIN, LOW);
        digitalWrite(RELAY2_PIN, LOW);
    }
    if (!isnan(pesoMoviCongelado) && pesoMoviCongelado > 700.0)
    {
        // digitalWrite(RELAY1_PIN, HIGH);
        digitalWrite(RELAY2_PIN, HIGH);
    }
}

// ENVIA VALORES DA TABELA RESIDUAL AO APLICATIVO
void sendResidualTable()
{
    ESP_LOGI("BLE", "Enviando TABELA_RESIDUAL via BLE...");
    String header = "TABELA_RESIDUAL\n";
    sendBLE(pCharacteristicTX, header);

    // Envia cada linha da tabela (altura + pesos)
    for (int i = 0; i < RESIDUAL_ROWS; i++)
    {
        String line = "H:" + String(heightResidual[i]) + ";";
        for (int j = 0; j < RESIDUAL_COLS; j++)
        {
            line += String(weightResidual[i][j]);
            if (j < RESIDUAL_COLS - 1)
                line += ","; // separa pesos com vírgula
        }
        sendBLE(pCharacteristicTX, line + "\n");
    }
    sendBLE(pCharacteristicTX, "FIM_TABELA\n");
}

// Salva a tabela completa (alturas + todos os pesos) na EEPROM
void saveResidualTableToPreferences()
{
    Preferences preferences;
    preferences.begin("residual", false); // modo escrita

    for (int i = 0; i < RESIDUAL_ROWS; i++)
    {
        String keyH = "H_" + String(i);
        preferences.putInt(keyH.c_str(), heightResidual[i]);
        for (int j = 0; j < RESIDUAL_COLS; j++)
        {
            String keyP = "P_" + String(i) + "_" + String(j);
            preferences.putInt(keyP.c_str(), weightResidual[i][j]);
        }
    }

    preferences.end();
}

void loadResidualTableFromPreferences()
{
    Preferences preferences;
    preferences.begin("residual", true); // modo leitura

    for (int i = 0; i < RESIDUAL_ROWS; i++)
    {
        String keyH = "H_" + String(i);
        heightResidual[i] = preferences.getInt(keyH.c_str(), heightResidual[i]);
        for (int j = 0; j < RESIDUAL_COLS; j++)
        {
            String keyP = "P_" + String(i) + "_" + String(j);
            weightResidual[i][j] = preferences.getInt(keyP.c_str(), weightResidual[i][j]);
        }
    }

    preferences.end();

    logMessage("Tabela Residual carregada da NVS (Preferences).");
}

// Processa comandos recebidos via BLE (SET:H, FIM_TABELA, GET_TABELA)
void processarComando(String value)
{
    if (value.length() == 0)
        return;

    if (value.startsWith("SET:H:"))
    {
        int sepIndex = value.indexOf(";");
        if (sepIndex > 0)
        {
            int altura = value.substring(6, sepIndex).toInt();
            String pesosStr = value.substring(sepIndex + 1);

            int idx = -1;

            // Verifica se a altura já existe
            for (int i = 0; i < RESIDUAL_ROWS; i++)
            {
                if (heightResidual[i] == altura)
                {
                    idx = i;
                    break;
                }
            }

            // Caso não exista, procura espaço vazio
            if (idx == -1)
            {
                for (int i = 0; i < RESIDUAL_ROWS; i++)
                {
                    if (heightResidual[i] == 0)
                    {
                        heightResidual[i] = altura;
                        idx = i;
                        break;
                    }
                }
            }

            // Se ainda não encontrou, erro
            if (idx == -1)
            {
                logMessage("ERROR:Não foi possível encontrar ou alocar altura " + String(altura));
                return;
            }

            // Converte string de pesos em valores inteiros
            int col = 0;
            while (pesosStr.length() > 0 && col < RESIDUAL_COLS)
            {
                int nextComma = pesosStr.indexOf(",");
                String pesoStr = (nextComma >= 0) ? pesosStr.substring(0, nextComma) : pesosStr;
                weightResidual[idx][col] = pesoStr.toInt();
                pesosStr = (nextComma >= 0) ? pesosStr.substring(nextComma + 1) : "";
                col++;
            }

            // Salvar essa linha diretamente nas Preferences
            Preferences preferences;
            preferences.begin("residual", false);
            String keyH = "H_" + String(idx);
            preferences.putInt(keyH.c_str(), heightResidual[idx]);

            for (int j = 0; j < RESIDUAL_COLS; j++)
            {
                String keyP = "P_" + String(idx) + "_" + String(j);
                preferences.putInt(keyP.c_str(), weightResidual[idx][j]);
            }
            preferences.end();
        }
        else
        {
            logMessage("ERROR:Linha mal formatada: " + value);
        }
    }
    else if (value == "FIM_TABELA")
    {
        // Comando que indica fim da tabela → salva tudo na EEPROM
        logMessage("TABELA RESIDUAL ATUALIZADA COM SUCESSO!");
        sendBLE(pCharacteristicTX, "TABELA_ATUALIZADA\n");
        saveResidualTableToPreferences();
    }
    else if (value == "GET_TABELA")
    {
        // Solicitação do app para reenviar a tabela
        sendBLE(pCharacteristicTX, "ACK: GET_TABELA\n");
        sendResidualTable();
    }
    else if (value == "GET_TENSAO")
    {
        int tensao_mV = (int)weightSensorValue;
        sendBLE(pCharacteristicTX, "TENSAO:" + String(tensao_mV) + "\n");
    }
    // função calibração
    else if (value.startsWith("SET_OFFSET:"))
    {
        int separador = value.indexOf(':');
        if (separador > 0)
        {
            String offsetStr = value.substring(separador + 1);
            float novoOffset = offsetStr.toFloat();

            // Aqui você pode aplicar o offset como quiser — exemplo simples:
            pesoSensorOffset = novoOffset;
            logMessage("Offset ajustado para: " + String(pesoSensorOffset));
        }
        else
        {
            logMessage("ERROR:Offset mal formatado: " + value);
        }
    }
    else
    {
        logMessage("ERROR:Comando BLE inválido: " + value);
    }
}

// Desliga Bluetooth caso usuário não se conectar dentro de 3 minutos
void bluetoothTimeOut(unsigned long currentTime)
{
    if (!isAuthenticated && !deviceConnected && currentTime - bleConnectionTime >= 180000) // 3 minutos em milissegundos
    {
        static bool bleDesligado = false;
        if (!bleDesligado)
        {
            ESP_LOGI("BLE", "Tempo limite excedido sem autenticação. Desligando BLE.");
            BLEDevice::deinit(true);
            bleDesligado = true;
        }
        return;
    }
}

void inicializarTabelaSeNecessario()
{
    Preferences preferences;
    preferences.begin("residual", true);

    // Checa se uma chave esperada está presente
    if (!preferences.isKey("H_0"))
    {
        preferences.end();
        saveResidualTableToPreferences();
        logMessage("Preferences inicializadas com tabela padrão.");
    }
    else
    {
        preferences.end();
        logMessage("Preferences já inicializadas.");
    }
}

bool cargaDetectada2s = false;
unsigned long tempoInicioCarga = 0;

void congelarPeso2Segundos()
{
    bool acimaDoBiestavel = (digitalRead(BIESTAVEL) == HIGH);
    const unsigned long DELAY_CONGELAR = 200UL;

    unsigned long agora = millis();

    if (!isnan(pesoCongelado2Segundos) && ultimoPesoValido < LIMIAR_ZERO)
    {
        pesoCongelado2Segundos = NAN;
        cargaDetectada2s = false;
        tempoInicioCarga = 0;
        primeiroPeso = true;
        podeCongelarPeso = true;
        ESP_LOGI("INFO", "Carga removida. Resetando pesoCongelado2Segundos.");
    }
    const float LIMIAR_PESO_CONGELAR_2S = 300.0f;

    if (!cargaDetectada2s && pesoSuavizado > LIMIAR_PESO_CONGELAR_2S && acimaDoBiestavel && !inSecondStage)

    {
        cargaDetectada2s = true;
        tempoInicioCarga = agora;
        pesoCongelado2Segundos = NAN;
    }

    if (cargaDetectada2s && (agora - tempoInicioCarga >= DELAY_CONGELAR) && acimaDoBiestavel)
    {
        if (isnan(pesoCongelado2Segundos))
        {
            pesoCongelado2Segundos = pesoSuavizado;
            ESP_LOGI("OK", "Peso congelado após 2s: %.1f", pesoCongelado2Segundos);
        }
    }
}

void setup()
{
    // Serial.begin is not needed; ESP_LOGx macros initialize the UART.

    BLEDevice::init("EMPILHADEIRA"); // nome do dispositvo ou identificação da empilhadeira
    BLEDevice::setMTU(517);
    bluetoothConnection();
    // Use ESP_LOGI, which is better than Serial for ESP-IDF projects
    ESP_LOGI(TAG, "--- PWM Capture and Speed Estimator Test ---");

    // 1. Initialize the hardware component to capture the PWM signal
    init_pwm_capture(PWM_ELEV);

    // 2. Initialize the estimator component, which starts its background task
    init_speed_estimator();

    ESP_LOGI(TAG, "Components initialized. Starting measurement loop...");
    bleLigado = true;
    tempoInicialBLE = millis();

    inicializarTabelaSeNecessario();
    loadResidualTableFromPreferences();

    pinMode(IND_NA, INPUT);
    pinMode(IND_NF, INPUT);
    pinMode(ENC_A, INPUT);
    pinMode(ENC_B, INPUT);
    pinMode(BIESTAVEL, INPUT);

    pinMode(BUZZER_PIN, OUTPUT);
    pinMode(RELAY1_PIN, OUTPUT); // Tração
    pinMode(RELAY2_PIN, OUTPUT); // Elevação

    digitalWrite(RELAY1_PIN, LOW);
    digitalWrite(RELAY2_PIN, LOW);
    pinMode(PINO_5V, OUTPUT);
    pinMode(PINO_12V, OUTPUT);
    digitalWrite(PINO_5V, HIGH);
    digitalWrite(PINO_12V, HIGH);
    FastLED.addLeds<WS2811, DATA_PIN, BRG>(leds, NUM_LEDS);
    FastLED.setBrightness(maxBrightness);

    SPI.begin(SCLK_PIN, MISO_PIN, MOSI_PIN, CS1_PIN);
    pinMode(CS1_PIN, OUTPUT);
    digitalWrite(CS1_PIN, HIGH);
    pinMode(CS0_PIN, OUTPUT);
    digitalWrite(CS0_PIN, HIGH);
    pinMode(PWM_ELEV, INPUT);
    digitalWrite(RELAY1_PIN, LOW);
    digitalWrite(RELAY2_PIN, LOW);
    attachInterrupt(digitalPinToInterrupt(ENC_A), encoderA, RISING);
    attachInterrupt(digitalPinToInterrupt(ENC_B), encoderB, RISING);

    // O código abaixo visa realizar um primeiro cálculo do peso inicial
    checkLiftStage();
    readAnalogInputs();
    pesoSuavizado = calculateStaticWeight(weightSensorValue);
    valorPesoCongelado = pesoSuavizado;
    digitalWrite(PINO_5V, HIGH);
    digitalWrite(PINO_12V, HIGH);
}

void loop()
{
   bool pesoAlto = pesoSuavizado > 100.0f; // peso mínmo permitido com garfo elevado sem travar a máquina

    // === BLOQUEIO DE SEGURANÇA NA INICIALIZAÇÃO ===
    if (primeiraInicializacao)
    {
        bool garfoElevado = (digitalRead(BIESTAVEL) == HIGH);

        if (garfoElevado && pesoAlto)
        {
            bloqueioInicial = true;
            ESP_LOGW("SEGURANÇA", "Máquina iniciou com garfo elevado ou carga > 100kg.");
        }
        else if (garfoElevado)
        {
            primeiraInicializacao = false;
        }
    }

    if (bloqueioInicial)
    {
        bool garfoElevado = (digitalRead(BIESTAVEL) == HIGH);

        if (!garfoElevado && !pesoAlto)
        {
            bloqueioInicial = false;
            ESP_LOGI("SEGURANÇA", "Condições normais detectadas. Máquina desbloqueada.");
        }
    }

    checkLiftStage();
    readAnalogInputs();

    unsigned long currentTime = millis();
    handlePWMState();

    bluetoothTimeOut(currentTime); // Desliga Bluetooth caso usuário não se conectar dentro de 3 minutos

    if (pwmElevState)
        processPesoEmMovimento();
    else
        coletandoMovimento = pesoMoviJaCongelado = false;

    bool movimentoRecente = pwmElevState && (currentTime - lastPWMStartTime < DELAY_MOVEMENT_START);
    int currentInterval = pwmElevState ? ANALOG_READ_FREQ_MOVING : ANALOG_READ_FREQ_STATIC;

    float velocidade = calcularVelocidade();  // mm/s
    pBar = convertMvToBar(weightSensorValue); // bar a partir de mV

    float velocidadeEncoder = calcularVelocidade();
    float duty_cycle = get_pwm_duty_cycle();
    float estimated_speed = get_estimated_speed();
    static float velocidadeParaCalculo; // This will hold the correct speed to use

    // Choose which speed is valid for the current stage
    if (inSecondStage) {
        velocidadeParaCalculo = velocidadeEncoder;
    } else {
        velocidadeParaCalculo = estimated_speed;
    }
    
    pBar = convertMvToBar(weightSensorValue);


    const unsigned long DELAY_BEFORE_STATIC_MS = 500;
    static unsigned long tempoParadaDetectada = 0;
    bool usarCalculoDinamico;

    if (pwmElevState || fabs(velocidadeParaCalculo) > LIMIAR_SPEED) {
        usarCalculoDinamico = true;
        tempoParadaDetectada = 0;
    } else {
        if (tempoParadaDetectada == 0) {
            tempoParadaDetectada = currentTime;
        }
        if (currentTime - tempoParadaDetectada >= DELAY_BEFORE_STATIC_MS) {
            usarCalculoDinamico = false;
        } else {
            usarCalculoDinamico = true;
        }
    }

    // 3. PERFORM FINAL WEIGHT CALCULATION (WITH YOUR "FREEZE" LOGIC RE-INTEGRATED)
    static bool pesoCongelado = false; // Flag to control the freeze
    // 'valorPesoCongelado' is your existing global variable

    if (usarCalculoDinamico) {
        // We are moving, so calculate dynamic weight.
        pesoAtual = calculateDynamicWeight(pBar, velocidadeParaCalculo);
        // IMPORTANT: Reset the freeze flag so we can capture a new static weight next time.
        pesoCongelado = false; 
    } else {
        // We are static. Apply the freeze logic.
        if (!pesoCongelado) {
            // If the weight isn't frozen yet, calculate it ONCE and freeze it.
            valorPesoCongelado = calculateStaticWeight(weightSensorValue);
            pesoCongelado = true; // Set the flag
            ESP_LOGI("PESO", "Static weight has been frozen at: %.1f", valorPesoCongelado);
            pesoAtual = valorPesoCongelado; // Use the newly frozen value immediately
        } else {
            // The weight is already frozen, so just keep using that stable value.
            pesoAtual = valorPesoCongelado;
        }
    }

        // if (!isnan(pesoCongelado2Segundos))
        // {
        //     pesoSeguro = pesoCongelado2Segundos;
        // }
        // else
        // {
        //     pesoSeguro = pesoSuavizado;
        // }

        bool cargaPegaNoSegundoEstagio = false;
        float MINIMO_CARGA = 300;

        if (!cargaDetectada && pesoSuavizado > MINIMO_CARGA)
        {
            cargaDetectada = true;
            cargaPegaNoSegundoEstagio = inSecondStage;
        }
        if (pesoSuavizado < LIMIAR_ZERO)
        {
            cargaDetectada = false;
            cargaPegaNoSegundoEstagio = false;
        }

        //    if (cargaPegaNoSegundoEstagio)
        if (inSecondStage)
        {
            // Carga foi pega no 2º estágio → usar suavizado até estabilizar
            // if (!isnan(pesoEstabilizado))
            // {
            //     pesoSeguro = pesoEstabilizado;
            // }
            // else
            // {
            pesoSeguro = pesoSuavizado;
            // }
        }
        else
        {
            // Carga foi pega no 1º estágio → só congelado2s ou suavizado
            if (!isnan(pesoCongelado2Segundos))
            {
                pesoSeguro = pesoCongelado2Segundos;
            }
            else
            {
                pesoSeguro = pesoSuavizado;
            }
        }

        if (pesoAtual < 0)
            pesoAtual = 0;
        currentWeight = pesoAtual;
        if (pesoAtual < 0)
            pesoAtual = 0;

        if (!movimentoRecente && (currentTime - lastTimeAnalogRead >= currentInterval))
        {
            if (!isnan(pesoAtual))
                ultimoPesoValido = pesoAtual;
            processPesoEstabilizado();
            exibirStatus(pesoAtual, duty_cycle, estimated_speed);

//            float pesoParaMostrar = isnan(pesoEstabilizado) ? ultimoPesoValido : pesoEstabilizado;
            pesoSuavizado = primeiroPeso ? ultimoPesoValido : ALPHA * ultimoPesoValido + (1.0 - ALPHA) * pesoSuavizado;
            primeiroPeso = false;
            lastTimeAnalogRead = currentTime;
            congelarPesoPorCargaComDelay();
            heightLimit = interpolateResidualHeight((int)pesoSeguro, 600) - MARGEM_SEGURANCA_MM;
        }
        congelarPeso2Segundos();

        updatePositionMm();

        if (bloqueioInicial)
        {
            piscarLedVinho();
        }
        else
        {
            updateLedVisual(pesoSeguro);
        }

        bluetoothReceiveData(); // Recebe os valores no aplicativo via bluetooth
        applySafetyRestrictions(pesoSeguro);
        applyFinalRelayState();
        buzzerControl();
        updateElevacaoParada();

        unsigned long ultimoStatusMillis = 0;
        const unsigned long INTERVALO_STATUS_MS = 8000;
        if (millis() - ultimoStatusMillis >= INTERVALO_STATUS_MS)
        {
            ultimoStatusMillis = millis();
        }
    }

    // =========================== 13 -  FUNÇÃO PRINCIPAL (MAIN) ===========================
    void mainTaskLoop(void *pvParameters)
    {
        for (;;)
        {
            loop();
            vTaskDelay(1); // Ajuste o atraso conforme necessário
        }
    }

    extern "C" void app_main(void)
    {
        ESP_LOGI(TAG, "Application has started! CPU Freq: %d MHz", CONFIG_ESP_DEFAULT_CPU_FREQ_MHZ);

        // The Arduino component provides this 'initArduino()' function.
        // It sets up the timers and other peripherals needed for Arduino
        // functions like millis(), delay(), etc. to work.
        initArduino();
        // uart_config_t uart_config = {
        //     .baud_rate = 115200,  // <--- CHECK THIS VALUE!  Common values: 9600, 115200, 230400
        //     .data_bits = UART_DATA_8_BITS,
        //     .parity    = UART_PARITY_DISABLE,
        //     .stop_bits = UART_STOP_BITS_1,
        //     .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
        //     .source_clk = UART_SCLK_DEFAULT,
        // };
        // uart_driver_install(UART_NUM_0, 1024 * 2, 0, 0, NULL, 0); // Initialize UART driver
        // esp_vfs_dev_uart_use_driver(UART_NUM_0);            // Install UART driver for printf

        // Now, we simply call the standard Arduino setup() and loop() functions.
        setup();

        // while loop comment to substitute to a taskWrapper
        // while (1)
        // {
        //     loop();
        //     // We can yield to other tasks if needed, though for a simple
        //     // single-task loop, this isn't strictly necessary.
        //     vTaskDelay(1);
        // }
        xTaskCreate(
            mainTaskLoop,             // The function to run
            "Main Loop Task Wrapper", // Name for debugging
            8192,                     // Stack size
            NULL,                     // No parameters
            1,                        // Priority
            NULL                      // No handle
        );
        vTaskDelete(NULL); // NULL means "delete the task that is currently running"
    }