#include <stdio.h>
#include <string.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"

#include "nvs.h"
#include "nvs_flash.h"

#include "esp_log.h"
#include "esp_bt.h"
#include "esp_bt_main.h"
#include "esp_gap_bt_api.h"
#include "esp_bt_device.h"
#include "esp_spp_api.h"

#include "HUB75.h"

#include "imgFleX.h"
#include "imgAltM.h"
#include "imgCint.h"
#include "imgVelM.h"

#include "Pines.h"

#include "Numeros.h"

#define TAM 768 // 768 uint32

DMA_ATTR uint32_t graphicsBufferUU[TAM];
DMA_ATTR uint32_t graphicsBufferUD[TAM];
DMA_ATTR uint32_t graphicsBufferDU[TAM];
DMA_ATTR uint32_t graphicsBufferDD[TAM];

// ---------------------
// | graphicsBufferUU  | 
// ---------------------
// | graphicsBufferUD  | 
// ---------------------
// | graphicsBufferDU  | 
// ---------------------
// | graphicsBufferDD  |
// ---------------------

#define VelM
//#define AltM
//#define Cint
//#define FleX

#define TAG "SMVCA_PGM"
#define SERVER_NAME "SMVCA_SERVER"

#ifdef VelM
#define DEVICE_NAME "SMVCA_PGM_VELM"
uint8_t Vel_Canal_Izquierdo = 60;
uint8_t Vel_Canal_Derecho   = 40;
#endif

#ifdef AltM
#define DEVICE_NAME "SMVCA_PGM_ALTM"
uint16_t Altura_Max = 34;
#endif

#ifdef FleX
#define DEVICE_NAME "SMVCA_PGM_FLEX"
#define FF  0
#define FX  1
#define XF  2
#define XX  3
uint8_t F_X = FF;
#endif

char Clave[5] = "ABCD";

#if defined(VelM) || defined(AltM) || defined(AltM) 

static const esp_spp_mode_t esp_spp_mode = ESP_SPP_MODE_CB;
static const bool esp_spp_enable_l2cap_ertm = true;

static const esp_spp_sec_t sec_mask = ESP_SPP_SEC_AUTHENTICATE;
static const esp_spp_role_t role_slave = ESP_SPP_ROLE_SLAVE;

#endif

static void HUB75_task(void *);

#if defined(VelM) || defined(AltM) || defined(AltM) 
static void func_clave(void);
#endif 

#ifdef VelM
static void func_VelM(void);
#endif

#ifdef AltM
static void func_AltM(void);
#endif

#ifdef FleX
static void func_FX(void);
#endif

uint8_t reload = 0;

#if defined(VelM) || defined(AltM) || defined(AltM) 
static char *bda2str(uint8_t *, char *, size_t);
static void esp_spp_cb(esp_spp_cb_event_t, esp_spp_cb_param_t *);
static void esp_bt_gap_cb(esp_bt_gap_cb_event_t, esp_bt_gap_cb_param_t *);
#endif 
static void initGraphicsBuffer(void);

void app_main(void)
{
    TaskHandle_t HUB75_Handle = NULL;

#if defined(VelM) || defined(AltM) || defined(AltM) 
    char bda_str[18] = {0};
#endif

    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND)
    {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK(ret);


#if defined(VelM) || defined(AltM) || defined(AltM) 
    func_clave();
#endif

#ifdef VelM
    func_VelM();
#endif

#ifdef AltM
    func_AltM();
#endif

#ifdef FleX
    func_FX();
#endif

    initGraphicsBuffer();

#if defined(VelM) || defined(AltM) || defined(AltM) 
    ESP_ERROR_CHECK(esp_bt_controller_mem_release(ESP_BT_MODE_BLE));

    esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT();
    if ((ret = esp_bt_controller_init(&bt_cfg)) != ESP_OK)
    {
        ESP_LOGE(TAG, "%s inicio del controlador fallo: %s\n", __func__, esp_err_to_name(ret));
        return;
    }

    if ((ret = esp_bt_controller_enable(ESP_BT_MODE_CLASSIC_BT)) != ESP_OK)
    {
        ESP_LOGE(TAG, "%s habilitacion del controlador fallo: %s\n", __func__, esp_err_to_name(ret));
        return;
    }

    if ((ret = esp_bluedroid_init()) != ESP_OK)
    {
        ESP_LOGE(TAG, "%s inicio del bluedroid fallo: %s\n", __func__, esp_err_to_name(ret));
        return;
    }

    if ((ret = esp_bluedroid_enable()) != ESP_OK)
    {
        ESP_LOGE(TAG, "%s habilitación del bluedroid fallo: %s\n", __func__, esp_err_to_name(ret));
        return;
    }

    if ((ret = esp_bt_gap_register_callback(esp_bt_gap_cb)) != ESP_OK)
    {
        ESP_LOGE(TAG, "%s registro del GAP fallo: %s\n", __func__, esp_err_to_name(ret));
        return;
    }

    if ((ret = esp_spp_register_callback(esp_spp_cb)) != ESP_OK)
    {
        ESP_LOGE(TAG, "%s registro del SPP fallo: %s\n", __func__, esp_err_to_name(ret));
        return;
    }

    esp_spp_cfg_t bt_spp_cfg = {
        .mode = esp_spp_mode,
        .enable_l2cap_ertm = esp_spp_enable_l2cap_ertm,
        .tx_buffer_size = 0, /* Only used for ESP_SPP_MODE_VFS mode */
    };
    if ((ret = esp_spp_enhanced_init(&bt_spp_cfg)) != ESP_OK)
    {
        ESP_LOGE(TAG, "%s inicio del SPP fallo: %s\n", __func__, esp_err_to_name(ret));
        return;
    }

#if (CONFIG_BT_SSP_ENABLED == true)
    /* Set default parameters for Secure Simple Pairing */
    esp_bt_sp_param_t param_type = ESP_BT_SP_IOCAP_MODE;
    esp_bt_io_cap_t iocap = ESP_BT_IO_CAP_IO;
    esp_bt_gap_set_security_param(param_type, &iocap, sizeof(uint8_t));
#endif

    esp_bt_pin_type_t pin_type = ESP_BT_PIN_TYPE_VARIABLE;
    esp_bt_pin_code_t pin_code;
    esp_bt_gap_set_pin(pin_type, 0, pin_code);

    ESP_LOGI(TAG, "Direccion:[%s]", bda2str((uint8_t *)esp_bt_dev_get_address(), bda_str, sizeof(bda_str)));
#endif

    xTaskCreatePinnedToCore(HUB75_task, "HUB75_task", 4096, NULL, 10, &HUB75_Handle, 1);

    while(true)
    {

        if(reload)
        {
            reload = 0;
            initGraphicsBuffer();
        }
        
        vTaskDelay(pdMS_TO_TICKS(1000));
    }
}

static void initGraphicsBuffer(void)
{
#ifdef VelM
    memcpy(graphicsBufferUU, imgVelM_UU, TAM * 4);  // 768 * 4 = 3072 Bytes
    memcpy(graphicsBufferUD, imgVelM_UD, TAM * 4);
    memcpy(graphicsBufferDU, imgVelM_DU, TAM * 4);
    memcpy(graphicsBufferDD, imgVelM_DD, TAM * 4);
    
    uint8_t VIM = Vel_Canal_Izquierdo / 10;
    uint8_t VIL = Vel_Canal_Izquierdo % 10;
    uint8_t VDM = Vel_Canal_Derecho / 10;
    uint8_t VDL = Vel_Canal_Derecho % 10;
    
    for(uint8_t j = 0; j < 24; j++)
    {
        graphicsBufferUD[ 3 + 0 + (8 + j) * 24] = (Num[VIM * 144 + 0 + 3 * j     ]      );
        graphicsBufferUD[ 3 + 1 + (8 + j) * 24] = (Num[VIM * 144 + 1 + 3 * j     ]      );
        graphicsBufferUD[ 3 + 2 + (8 + j) * 24] = (Num[VIM * 144 + 2 + 3 * j     ]      );
        graphicsBufferUD[ 6 + 0 + (8 + j) * 24] = (Num[VIL * 144 + 0 + 3 * j     ] >> 16) | 0xFFFF0000;
        graphicsBufferUD[ 6 + 1 + (8 + j) * 24] = (Num[VIL * 144 + 0 + 3 * j     ] << 16) | (Num[VIL * 144 + 1 + 3 * j     ] >> 16);
        graphicsBufferUD[ 6 + 2 + (8 + j) * 24] = (Num[VIL * 144 + 1 + 3 * j     ] << 16) | (Num[VIL * 144 + 2 + 3 * j     ] >> 16);
        graphicsBufferUD[ 6 + 3 + (8 + j) * 24] = (Num[VIL * 144 + 2 + 3 * j     ] << 16) | (graphicsBufferUD[ 6 + 3 + (8 + j) * 24] & 0x0000FFFF); 
        graphicsBufferUD[14 + 0 + (8 + j) * 24] = (Num[VDM * 144 + 0 + 3 * j     ] >> 16) | (graphicsBufferUD[14 + 0 + (8 + j) * 24] & 0xFFFF0000);
        graphicsBufferUD[14 + 1 + (8 + j) * 24] = (Num[VDM * 144 + 0 + 3 * j     ] << 16) | (Num[VDM * 144 + 1 + 3 * j     ] >> 16);
        graphicsBufferUD[14 + 2 + (8 + j) * 24] = (Num[VDM * 144 + 1 + 3 * j     ] << 16) | (Num[VDM * 144 + 2 + 3 * j     ] >> 16);
        graphicsBufferUD[14 + 3 + (8 + j) * 24] = (Num[VDM * 144 + 2 + 3 * j     ] << 16) | 0x0000FFFF;
        graphicsBufferUD[18 + 0 + (8 + j) * 24] = (Num[VDL * 144 + 0 + 3 * j     ]      );
        graphicsBufferUD[18 + 1 + (8 + j) * 24] = (Num[VDL * 144 + 1 + 3 * j     ]      );
        graphicsBufferUD[18 + 2 + (8 + j) * 24] = (Num[VDL * 144 + 2 + 3 * j     ]      );
  
        graphicsBufferDU[ 3 + 0 + (    j) * 24] = (Num[VIM * 144 + 0 + 3 * j + 72]      );
        graphicsBufferDU[ 3 + 1 + (    j) * 24] = (Num[VIM * 144 + 1 + 3 * j + 72]      );
        graphicsBufferDU[ 3 + 2 + (    j) * 24] = (Num[VIM * 144 + 2 + 3 * j + 72]      );
        graphicsBufferDU[ 6 + 0 + (    j) * 24] = (Num[VIL * 144 + 0 + 3 * j + 72] >> 16) | 0xFFFF0000;
        graphicsBufferDU[ 6 + 1 + (    j) * 24] = (Num[VIL * 144 + 0 + 3 * j + 72] << 16) | (Num[VIL * 144 + 1 + 3 * j + 72] >> 16);
        graphicsBufferDU[ 6 + 2 + (    j) * 24] = (Num[VIL * 144 + 1 + 3 * j + 72] << 16) | (Num[VIL * 144 + 2 + 3 * j + 72] >> 16);
        graphicsBufferDU[ 6 + 3 + (    j) * 24] = (Num[VIL * 144 + 2 + 3 * j + 72] << 16) | (graphicsBufferDU[ 6 + 3 + (    j) * 24] & 0x0000FFFF); 
        graphicsBufferDU[14 + 0 + (    j) * 24] = (Num[VDM * 144 + 0 + 3 * j + 72] >> 16) | (graphicsBufferDU[14 + 0 + (    j) * 24] & 0xFFFF0000);
        graphicsBufferDU[14 + 1 + (    j) * 24] = (Num[VDM * 144 + 0 + 3 * j + 72] << 16) | (Num[VDM * 144 + 1 + 3 * j + 72] >> 16);
        graphicsBufferDU[14 + 2 + (    j) * 24] = (Num[VDM * 144 + 1 + 3 * j + 72] << 16) | (Num[VDM * 144 + 2 + 3 * j + 72] >> 16);
        graphicsBufferDU[14 + 3 + (    j) * 24] = (Num[VDM * 144 + 2 + 3 * j + 72] << 16) | 0x0000FFFF;
        graphicsBufferDU[18 + 0 + (    j) * 24] = (Num[VDL * 144 + 0 + 3 * j + 72]      );
        graphicsBufferDU[18 + 1 + (    j) * 24] = (Num[VDL * 144 + 1 + 3 * j + 72]      );
        graphicsBufferDU[18 + 2 + (    j) * 24] = (Num[VDL * 144 + 2 + 3 * j + 72]      );
    }
#endif

#ifdef AltM
    memcpy(graphicsBufferUU, imgAltM_UU, TAM * 4);  // 768 * 4 = 3072 Bytes
    memcpy(graphicsBufferUD, imgAltM_UD, TAM * 4);
    memcpy(graphicsBufferDU, imgAltM_DU, TAM * 4);
    memcpy(graphicsBufferDD, imgAltM_DD, TAM * 4);

    uint8_t AM = Altura_Max / 10;
    uint8_t AL = Altura_Max % 10;

    for(uint8_t j = 0; j < 24; j++)
    {
        graphicsBufferUD[ 3 + 0 + (8 + j) * 24] = (Num[AM * 144 + 0 + 3 * j     ]      );
        graphicsBufferUD[ 3 + 1 + (8 + j) * 24] = (Num[AM * 144 + 1 + 3 * j     ]      );
        graphicsBufferUD[ 3 + 2 + (8 + j) * 24] = (Num[AM * 144 + 2 + 3 * j     ]      );
        graphicsBufferUD[ 6 + 0 + (8 + j) * 24] = (Num[AL * 144 + 0 + 3 * j     ] >> 16) | 0xFFFF0000;
        graphicsBufferUD[ 6 + 1 + (8 + j) * 24] = (Num[AL * 144 + 0 + 3 * j     ] << 16) | (Num[AL * 144 + 1 + 3 * j     ] >> 16);
        graphicsBufferUD[ 6 + 2 + (8 + j) * 24] = (Num[AL * 144 + 1 + 3 * j     ] << 16) | (Num[AL * 144 + 2 + 3 * j     ] >> 16);
        graphicsBufferUD[ 6 + 3 + (8 + j) * 24] = (Num[AL * 144 + 2 + 3 * j     ] << 16) | (graphicsBufferUD[ 6 + 3 + (8 + j) * 24] & 0x0000FFFF); 
     
        graphicsBufferDU[ 3 + 0 + (    j) * 24] = (Num[AM * 144 + 0 + 3 * j + 72]      );
        graphicsBufferDU[ 3 + 1 + (    j) * 24] = (Num[AM * 144 + 1 + 3 * j + 72]      );
        graphicsBufferDU[ 3 + 2 + (    j) * 24] = (Num[AM * 144 + 2 + 3 * j + 72]      );
        graphicsBufferDU[ 6 + 0 + (    j) * 24] = (Num[AL * 144 + 0 + 3 * j + 72] >> 16) | 0xFFFF0000;
        graphicsBufferDU[ 6 + 1 + (    j) * 24] = (Num[AL * 144 + 0 + 3 * j + 72] << 16) | (Num[AL * 144 + 1 + 3 * j + 72] >> 16);
        graphicsBufferDU[ 6 + 2 + (    j) * 24] = (Num[AL * 144 + 1 + 3 * j + 72] << 16) | (Num[AL * 144 + 2 + 3 * j + 72] >> 16);
        graphicsBufferDU[ 6 + 3 + (    j) * 24] = (Num[AL * 144 + 2 + 3 * j + 72] << 16) | (graphicsBufferDU[ 6 + 3 + (    j) * 24] & 0x0000FFFF);
    }
    graphicsBufferDU[462] &= 0x70077777;
    graphicsBufferDU[486] &= 0x00007777;
    graphicsBufferDU[510] &= 0x00007777;
    graphicsBufferDU[534] &= 0x70077777;
#endif

#ifdef Cint
    memcpy(graphicsBufferUU, imgCint_UU, TAM * 4);  // 768 * 4 = 3072 Bytes
    memcpy(graphicsBufferUD, imgCint_UD, TAM * 4);
    memcpy(graphicsBufferDU, imgCint_DU, TAM * 4);
    memcpy(graphicsBufferDD, imgCint_DD, TAM * 4);
#endif

#ifdef FleX
    switch (F_X)
    {
    case FF:
        memcpy(graphicsBufferUU, img_FleX_FF_UU, TAM * 4);  // 768 * 4 = 3072 Bytes
        memcpy(graphicsBufferUD, img_FleX_FF_UD, TAM * 4);
        memcpy(graphicsBufferDU, img_FleX_FF_DU, TAM * 4);
        memcpy(graphicsBufferDD, img_FleX_FF_DD, TAM * 4);
        break;
    case FX:
        memcpy(graphicsBufferUU, img_FleX_FX_UU, TAM * 4);  // 768 * 4 = 3072 Bytes
        memcpy(graphicsBufferUD, img_FleX_FX_UD, TAM * 4);
        memcpy(graphicsBufferDU, img_FleX_FX_DU, TAM * 4);
        memcpy(graphicsBufferDD, img_FleX_FX_DD, TAM * 4);
        break;
    case XF:
        memcpy(graphicsBufferUU, img_FleX_XF_UU, TAM * 4);  // 768 * 4 = 3072 Bytes
        memcpy(graphicsBufferUD, img_FleX_XF_UD, TAM * 4);
        memcpy(graphicsBufferDU, img_FleX_XF_DU, TAM * 4);
        memcpy(graphicsBufferDD, img_FleX_XF_DD, TAM * 4);
        break;
    case XX:
        memcpy(graphicsBufferUU, img_FleX_XX_UU, TAM * 4);  // 768 * 4 = 3072 Bytes
        memcpy(graphicsBufferUD, img_FleX_XX_UD, TAM * 4);
        memcpy(graphicsBufferDU, img_FleX_XX_DU, TAM * 4);
        memcpy(graphicsBufferDD, img_FleX_XX_DD, TAM * 4);
        break;
    }
#endif
}

static void HUB75_task(void *arg)
{
    HUB75_init();

    while (1)
    {
        HUB75_displayBuffer_Color64(graphicsBufferUU, graphicsBufferUD, graphicsBufferDU, graphicsBufferDD);
        vTaskDelay(pdMS_TO_TICKS(1));
    }
}

#if defined(VelM) || defined(AltM) || defined(AltM) 

static void func_clave(void)
{
    esp_err_t err;
    nvs_handle_t my_handle;
    char clave_temp[5];

    err = nvs_open("storage", NVS_READWRITE, &my_handle);

    if (err != ESP_OK)
        ESP_LOGE(TAG, "Error abriendo NVS (%s)", esp_err_to_name(err));
    else
    {
        size_t tam = 0;
        err = nvs_get_blob(my_handle, "clave", NULL, &tam);
        if ((err != ESP_OK) && (err != ESP_ERR_NVS_NOT_FOUND))
            ESP_LOGE(TAG, "Error chequear clave (%s)", esp_err_to_name(err));

        if (tam > 0)
        {
            err = nvs_get_blob(my_handle, "clave", clave_temp, &tam);
            if (err != ESP_OK)
                ESP_LOGE(TAG, "Error leer (%s)", esp_err_to_name(err));
            else
            {
                clave_temp[4] = '\0';
                strcpy(Clave, clave_temp);
            }
        }

        nvs_set_blob(my_handle, "clave", clave_temp, tam);
        nvs_commit(my_handle);
        nvs_close(my_handle);
    }

    Clave[4] = '\0';
}

#ifdef VelM
static void func_VelM(void)
{
    esp_err_t err;
    nvs_handle_t my_handle;

    err = nvs_open("storage", NVS_READWRITE, &my_handle);

    if (err != ESP_OK)
        ESP_LOGE(TAG, "Error abriendo NVS (%s)", esp_err_to_name(err));
    else
    {
        uint8_t VD, VI;

        if(nvs_get_u8(my_handle, "VD", &VD) == ESP_OK)
            Vel_Canal_Derecho = VD;
        else
            nvs_set_i32(my_handle, "VD", Vel_Canal_Derecho);

        if(nvs_get_u8(my_handle, "VI", &VI) == ESP_OK)
            Vel_Canal_Izquierdo = VI;
        else
            nvs_set_i32(my_handle, "VI", Vel_Canal_Izquierdo);

        nvs_commit(my_handle);
        nvs_close(my_handle);
    }
}
#endif

#ifdef AltM
static void func_AltM(void)
{
    esp_err_t err;
    nvs_handle_t my_handle;

    err = nvs_open("storage", NVS_READWRITE, &my_handle);

    if (err != ESP_OK)
        ESP_LOGE(TAG, "Error abriendo NVS (%s)", esp_err_to_name(err));
    else
    {
        uint8_t AM;

        if(nvs_get_u8(my_handle, "AM", &AM) == ESP_OK)
            Altura_Max = AM;
        else
            nvs_set_i32(my_handle, "AM", Altura_Max);

        nvs_commit(my_handle);
        nvs_close(my_handle);
    }
}
#endif

#ifdef FleX
static void func_FX(void)
{
    esp_err_t err;
    nvs_handle_t my_handle;

    err = nvs_open("storage", NVS_READWRITE, &my_handle);

    if (err != ESP_OK)
        ESP_LOGE(TAG, "Error abriendo NVS (%s)", esp_err_to_name(err));
    else
    {
        uint8_t FX_T;

        if(nvs_get_u8(my_handle, "FX", &FX_T) == ESP_OK)
            F_X = FX_T;
        else
            nvs_set_i32(my_handle, "FX", F_X);

        nvs_commit(my_handle);
        nvs_close(my_handle);
    }
}
#endif

static char *bda2str(uint8_t *bda, char *str, size_t size)
{
    if (bda == NULL || str == NULL || size < 18)
    {
        return NULL;
    }

    uint8_t *p = bda;
    sprintf(str, "%02x:%02x:%02x:%02x:%02x:%02x",
            p[0], p[1], p[2], p[3], p[4], p[5]);
    return str;
}

static void esp_spp_cb(esp_spp_cb_event_t event, esp_spp_cb_param_t *param)
{
    switch (event)
    {
    case ESP_SPP_INIT_EVT:
        if (param->init.status == ESP_SPP_SUCCESS)
            esp_spp_start_srv(sec_mask, role_slave, 0, SERVER_NAME);
        break;
    case ESP_SPP_START_EVT:
        if (param->start.status == ESP_SPP_SUCCESS)
        {
            esp_bt_dev_set_device_name(DEVICE_NAME);
            esp_bt_gap_set_scan_mode(ESP_BT_CONNECTABLE, ESP_BT_GENERAL_DISCOVERABLE);
        }
        break;
    case ESP_SPP_DATA_IND_EVT:
        switch (param->data_ind.data[0])
        {
        case 'C':
#ifdef VelM
            // 0123456789
            // C6040ABCDF
            if (
                (param->data_ind.len != 12) ||
                (param->data_ind.data[1] < '0') || (param->data_ind.data[1] > '9') ||
                (param->data_ind.data[2] < '0') || (param->data_ind.data[2] > '9') ||
                (param->data_ind.data[3] < '0') || (param->data_ind.data[3] > '9') ||
                (param->data_ind.data[4] < '0') || (param->data_ind.data[4] > '9') ||
                (param->data_ind.data[5] != Clave[0]) ||
                (param->data_ind.data[6] != Clave[1]) ||
                (param->data_ind.data[7] != Clave[2]) ||
                (param->data_ind.data[8] != Clave[3]) ||
                (param->data_ind.data[9] != 'F') 
                )
            {
                esp_spp_write(param->data_ind.handle, 4, (uint8_t *)"E1  ");
                ESP_LOGE(TAG, "E1");
            }
            else
            {
                uint8_t err = 0;
                nvs_handle_t my_handle;
                char resp[18];
                uint8_t VI, VD;

                VI = (param->data_ind.data[1] - '0') * 10 + param->data_ind.data[2] - '0';
                VD = (param->data_ind.data[3] - '0') * 10 + param->data_ind.data[4] - '0';

                if(nvs_open("storage", NVS_READWRITE, &my_handle) != ESP_OK)
                    sprintf(resp, "E2  ");
                else
                {
                    if(nvs_set_u8(my_handle, "VD", VD) != ESP_OK) 
                        err = 1;
                    if(nvs_set_u8(my_handle, "VI", VI) != ESP_OK) 
                        err += 2;
                        
                    if(err < 3)
                    {
                        if(nvs_commit(my_handle) == ESP_OK)
                        {
                            nvs_close(my_handle);
                            if(err == 0)
                            {
                                sprintf(resp, "%02d%02d", VI, VD);
                                Vel_Canal_Izquierdo = VI;
                                Vel_Canal_Derecho   = VD;
                            }
                            else if(err == 1)
                            {
                                sprintf(resp, "%02d%02d", Vel_Canal_Izquierdo, VD);
                                Vel_Canal_Derecho   = VD;
                            }
                            else
                            {
                                sprintf(resp, "%02d%02d", VI, Vel_Canal_Derecho);
                                Vel_Canal_Izquierdo = VI;   
                            }
                            reload = 1;
                        }
                        else
                            sprintf(resp, "E3  ");
                    }
                    else
                        sprintf(resp, "E4  ");
                }

                esp_spp_write(param->data_ind.handle, 4, (uint8_t *)resp);
                ESP_LOGI(TAG, "%s", resp);
            }
#endif

#ifdef AltM
            // 01234567
            // C34ABCDF
            if (
                (param->data_ind.len != 10) ||
                (param->data_ind.data[1] < '0') || (param->data_ind.data[1] > '9') ||
                (param->data_ind.data[2] < '0') || (param->data_ind.data[2] > '9') ||
                (param->data_ind.data[3] != Clave[0]) ||
                (param->data_ind.data[4] != Clave[1]) ||
                (param->data_ind.data[5] != Clave[2]) ||
                (param->data_ind.data[6] != Clave[3]) ||
                (param->data_ind.data[7] != 'F') 
                )
            {
                esp_spp_write(param->data_ind.handle, 2, (uint8_t *)"E1");
                ESP_LOGE(TAG, "E1");
            }
            else
            {
                uint8_t err = 0;
                nvs_handle_t my_handle;
                char resp[18];
                uint8_t AM;

                AM = (param->data_ind.data[1] - '0') * 10 + param->data_ind.data[2] - '0';
                
                if(nvs_open("storage", NVS_READWRITE, &my_handle) != ESP_OK)
                    sprintf(resp, "E2");
                else
                {
                    if(nvs_set_u8(my_handle, "AM", AM) != ESP_OK) 
                        err = 1;
                    
                    if(err == 0)
                    {
                        if(nvs_commit(my_handle) == ESP_OK)
                        {
                            nvs_close(my_handle);
                            sprintf(resp, "%02d", AM);
                            Altura_Max = AM;
                            reload = 1;
                        }
                        else
                            sprintf(resp, "E3");
                    }
                    else
                        sprintf(resp, "E4");

                }

                esp_spp_write(param->data_ind.handle, 2, (uint8_t *)resp);
                ESP_LOGI(TAG, "%s", resp);
            }
#endif

#ifdef FleX
            // 01234567
            // CFXABCDF
            if (
                (param->data_ind.len != 10) ||
                ((param->data_ind.data[1] != 'F') && (param->data_ind.data[1] != 'X')) ||
                ((param->data_ind.data[2] != 'F') && (param->data_ind.data[2] != 'X')) ||
                (param->data_ind.data[3] != Clave[0]) ||
                (param->data_ind.data[4] != Clave[1]) ||
                (param->data_ind.data[5] != Clave[2]) ||
                (param->data_ind.data[6] != Clave[3]) ||
                (param->data_ind.data[7] != 'F') 
               )
            {
                esp_spp_write(param->data_ind.handle, 2, (uint8_t *)"E1");
                ESP_LOGE(TAG, "E1");
            }
            else
            {
                nvs_handle_t my_handle;
                char resp[18];
                uint8_t FX_T = 0;

                if(param->data_ind.data[1] == 'X')
                    FX_T += 2;
                if(param->data_ind.data[2] == 'X')
                    FX_T += 1;

                if(nvs_open("storage", NVS_READWRITE, &my_handle) != ESP_OK)
                    sprintf(resp, "E2");
                else
                {
                    if(nvs_set_u8(my_handle, "FX", FX_T) == ESP_OK) 
                    {
                        if(nvs_commit(my_handle) == ESP_OK)
                        {
                            nvs_close(my_handle);
                            sprintf(resp, "%c%c", param->data_ind.data[1], param->data_ind.data[2]);
                            F_X = FX_T;
                            reload = 1;
                        }
                        else 
                            sprintf(resp, "E3");
                    }
                    else
                        sprintf(resp, "E4");
                }
                      
                esp_spp_write(param->data_ind.handle, 2, (uint8_t *)resp);
                ESP_LOGI(TAG, "%s", resp);
            }
#endif
            break;
        case 'P':
            if (
                (param->data_ind.len != 12) ||
                (param->data_ind.data[1] < 'A') || (param->data_ind.data[1] > 'Z') ||
                (param->data_ind.data[2] < 'A') || (param->data_ind.data[2] > 'Z') ||
                (param->data_ind.data[3] < 'A') || (param->data_ind.data[3] > 'Z') ||
                (param->data_ind.data[4] < 'A') || (param->data_ind.data[4] > 'Z') ||
                (param->data_ind.data[5] != Clave[0]) ||
                (param->data_ind.data[6] != Clave[1]) ||
                (param->data_ind.data[7] != Clave[2]) ||
                (param->data_ind.data[8] != Clave[3]) ||
                (param->data_ind.data[9] != 'F')
                )
            {
                esp_spp_write(param->data_ind.handle, 4, (uint8_t *)"E1  ");
                ESP_LOGE(TAG, "E1  ");
            }
            else
            {
                uint8_t err = 0;
                nvs_handle_t my_handle;
                char resp[7];
                char clave_temp[5];

                clave_temp[0] = param->data_ind.data[1];
                clave_temp[1] = param->data_ind.data[2];
                clave_temp[2] = param->data_ind.data[3];
                clave_temp[3] = param->data_ind.data[4];

                if(nvs_open("storage", NVS_READWRITE, &my_handle) != ESP_OK)
                    err = 1;
                else
                {
                    if(nvs_set_blob(my_handle, "clave", clave_temp, 4) == ESP_OK) 
                    {
                        if(nvs_commit(my_handle) == ESP_OK)
                        {
                            nvs_close(my_handle);
                                Clave[0] = clave_temp[0];
                                Clave[1] = clave_temp[1];
                                Clave[2] = clave_temp[2];
                                Clave[3] = clave_temp[3];
                                Clave[4] = '\0';
                        }
                        else
                            err = 1;
                    }
                    else
                        err = 1;
                }

                if(err == 0)
                    sprintf(resp, "%c%c%c%c", Clave[0], Clave[1], Clave[2], Clave[3]);
                else
                    sprintf(resp, "E2  ");
                esp_spp_write(param->data_ind.handle, 6, (uint8_t *)resp);
                ESP_LOGI(TAG, "%s", resp);                
            }
            break;
        default:
            esp_spp_write(param->data_ind.handle, 4, (uint8_t *)"E3  ");
            ESP_LOGE(TAG, "E3  ");
        }
        break;
    default:
        break;
    }
}

static void esp_bt_gap_cb(esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *param)
{
    char bda_str[18] = {0};

    switch (event)
    {
    case ESP_BT_GAP_AUTH_CMPL_EVT:
    {
        if (param->auth_cmpl.stat == ESP_BT_STATUS_SUCCESS)
        {
            ESP_LOGI(TAG, "authentication success: %s bda:[%s]", param->auth_cmpl.device_name,
                     bda2str(param->auth_cmpl.bda, bda_str, sizeof(bda_str)));
        }
        else
        {
            ESP_LOGE(TAG, "authentication failed, status:%d", param->auth_cmpl.stat);
        }
        break;
    }
    case ESP_BT_GAP_PIN_REQ_EVT:
    {
        ESP_LOGI(TAG, "ESP_BT_GAP_PIN_REQ_EVT min_16_digit:%d", param->pin_req.min_16_digit);
        if (param->pin_req.min_16_digit)
        {
            ESP_LOGI(TAG, "Input pin code: 0000 0000 0000 0000");
            esp_bt_pin_code_t pin_code = {0};
            esp_bt_gap_pin_reply(param->pin_req.bda, true, 16, pin_code);
        }
        else
        {
            ESP_LOGI(TAG, "Input pin code: 1234");
            esp_bt_pin_code_t pin_code;
            pin_code[0] = '1';
            pin_code[1] = '2';
            pin_code[2] = '3';
            pin_code[3] = '4';
            esp_bt_gap_pin_reply(param->pin_req.bda, true, 4, pin_code);
        }
        break;
    }

#if (CONFIG_BT_SSP_ENABLED == true)
    case ESP_BT_GAP_CFM_REQ_EVT:
        ESP_LOGI(TAG, "ESP_BT_GAP_CFM_REQ_EVT Please compare the numeric value: %" PRIu32, param->cfm_req.num_val);
        esp_bt_gap_ssp_confirm_reply(param->cfm_req.bda, true);
        break;
    case ESP_BT_GAP_KEY_NOTIF_EVT:
        ESP_LOGI(TAG, "ESP_BT_GAP_KEY_NOTIF_EVT passkey:%" PRIu32, param->key_notif.passkey);
        break;
    case ESP_BT_GAP_KEY_REQ_EVT:
        ESP_LOGI(TAG, "ESP_BT_GAP_KEY_REQ_EVT Please enter passkey!");
        break;
#endif

    case ESP_BT_GAP_MODE_CHG_EVT:
        ESP_LOGI(TAG, "ESP_BT_GAP_MODE_CHG_EVT mode:%d bda:[%s]", param->mode_chg.mode,
                 bda2str(param->mode_chg.bda, bda_str, sizeof(bda_str)));
        break;

    default:
    {
        ESP_LOGI(TAG, "event: %d", event);
        break;
    }
    }
    return;
}

#endif