/* Sniffer example.
   This example code is in the Public Domain (or CC0 licensed, at your option.)

   Unless required by applicable law or agreed to in writing, this
   software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
   CONDITIONS OF ANY KIND, either express or implied.

   partial source www.ardisoft.com (http://www.ardisoft.com)
   modifié par xav yéyé tranquille
   version 4.0
  analyse suite de trames, correction perte de trames
  ATTENTION, pour que cette version fonctionne bien entre bt et wifi, il faut modifier le fichier sdkconfig.h
  il est situé en général là:
  C:\Users\monnomutilisateur\AppData\Local\Arduino15\packages\esp32\hardware\esp32\1.0.5\tools\sdk\include\config
*/

#include <byteswap.h>
#include "esp_wifi.h"
#include "nvs_flash.h"
#include <WiFi.h>
#include <BLEDevice.h>
#include <BLEServer.h>
//#include "esp_bt_device.h"

#define LED 2
#define ON HIGH
#define OFF LOW
//#define DEBUG

#define SERVICE_UUID        "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8"

// calcul du temps depuis dernière capture trame
unsigned long agetrame = millis();
unsigned long comptesecondes = millis();
//la trame reconstituée pour le BT
String trame = "";
boolean tramerecue = false;
// construire la trame à envoyer au BT
String ID_Balise = "...";
String LAT_home = "...";
String LON_home = "...";
String LAT_actu = "...";
String LON_actu = "...";
String HAU_actu = "...";
String ALT_actu = "...";
String VIT_actu = "...";
String DIR_actu = "...";
double codeinfo = 0;
String Wifi_RSSI = "...";
String Wifi_Canal = "...";
String Wifi_Rate = "...";
String Wifi_Bande = "...";
//mise en forme des données wifi pour la trame BT:
static const char* const WIFI_PHY_RATE[] =
{
  "1_Mbps_LP",
  "2_Mbps_LP",
  "5.5_Mbps_LP",
  "11_Mbps_LP",
  "",
  "2_Mbps_SP",
  "5.5_Mbps_SP",
  "11_Mbps_SP",
  "48_Mbps",
  "24_Mbps",
  "12_Mbps",
  "6_Mbps",
  "54_Mbps",
  "36_Mbps",
  "18_Mbps",
  "9_Mbps",
  "MCS0_with_long_GI,_6.5_Mbps_for_20MHz,_13.5_Mbps_for_40MHz",
  "MCS1_with_long_GI,_13_Mbps_for_20MHz,_27_Mbps_for_40MHz",
  "MCS2_with_long_GI,_19.5_Mbps_for_20MHz,_40.5_Mbps_for_40MHz",
  "MCS3_with_long_GI,_26_Mbps_for_20MHz,_54_Mbps_for_40MHz",
  "MCS4_with_long_GI,_39_Mbps_for_20MHz,_81_Mbps_for_40MHz",
  "MCS5_with_long_GI,_52_Mbps_for_20MHz,_108_Mbps_for_40MHz",
  "MCS6_with_long_GI,_58.5_Mbps_for_20MHz,_121.5_Mbps_for_40MHz",
  "MCS7_with_long_GI,_65_Mbps_for_20MHz,_135_Mbps_for_40MHz",
  "MCS7_with_short_GI,_72.2_Mbps_for_20MHz,_150_Mbps_for_40MHz",
  "250_Kbps_LORA",
  "500_Kbps_LORA",
};
static const char* const WIFI_CBW[] =
{
  "20MHz",
  "40MHz",
};

//partie BT
BLEServer *pServer = NULL;
BLECharacteristic * pCharacteristic;

bool deviceConnected = false;
class MyServerCallbacks: public BLEServerCallbacks
{
    void onConnect(BLEServer* pServer)
    {
      deviceConnected = true;
      //BLEDevice::stopAdvertising();
      //Serial.println("BT connecté");
    };

    void onDisconnect(BLEServer* pServer)
    {
      deviceConnected = false;
      BLEDevice::startAdvertising();
      //Serial.println("BT NON connecté");
    }
};

void InitBT()
{
  BLEDevice::init("recepteur_DGAC_XAV");
  BLEDevice::setMTU(256);
  BLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_ADV ); //marche pas partout

  //esp_bt_dev_set_device_name("recepteur_DGAC_XAV");

  BLEServer *pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());
  BLEService *pService = pServer->createService(SERVICE_UUID);
  pCharacteristic = pService->createCharacteristic(
                      CHARACTERISTIC_UUID,
                      BLECharacteristic::PROPERTY_READ |
                      BLECharacteristic::PROPERTY_WRITE
                    );
  pService->start();
  // BLEAdvertising *pAdvertising = pServer->getAdvertising();  // this still is working for backward compatibility
  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID(SERVICE_UUID);
  pAdvertising->setScanResponse(true);
  pAdvertising->setMinPreferred(0x06);  // functions that help with iPhone connections issue //0x06
  pAdvertising->setMaxPreferred(0x12);  // 0x12
  BLEDevice::startAdvertising();
}

//sniffeur wifi
// wifi channel doit être 6
const int wifi_channel = 6;
static uint16_t num_trame = 0;
static const char *TAG = "sniffer";

#define OUI_VER 0x01355C6A
#define B_VERSION 0x01
#define B_IDENT_FR 0x02
#define B_IDENT_ANSI 0x03
#define B_CUR_LAT 0x04
#define B_CUR_LON 0x05
#define B_CUR_ALT 0x06
#define B_CUR_HAU 0x07
#define B_STA_LAT 0x08
#define B_STA_LON 0x09
#define B_CUR_VIT 0x0A
#define B_CUR_DIR 0x0B
#define B_CODE_INFO 0x0C

/** structure for Beacon Management Frame 802.11 + mandatory fields*/
typedef struct
{
  uint8_t subtype;              /**<  Frame subtype*/
  uint8_t type;               /**<  Frame typa*/
  uint16_t duration;              /**<  */
  uint8_t receiver_address[6];        /**<  Receiver mac-address*/
  uint8_t transmitter_address[6];       /**<  Transmitter mac-address*/
  uint8_t BSS_Id[6];              /**<  */
  uint16_t sequence_frag;           /**< 4 bits LSB frag, 12 bits MSB sequence sequence_frag = (0x000F & (frag)) | ( 0x0FFF & sequence<<4) */
  uint64_t timestamp;             /**<  Timestamp of frame*/
  uint16_t beacon_interval;         /** Interval between beacon frame */
  uint16_t capability;            /**mandatory fields */
  uint8_t ssid_element_id;          /** SSID Element ID */
  uint8_t ssid_length;            /** SSID length */
  uint8_t ssid_value[]; /** SSID value*/

} __attribute((__packed__)) frame_header_t;

typedef struct
{
  uint8_t type;       /**<  */
  uint8_t length;       /**<  */
  uint8_t payload[];      /**<  */
} __attribute((__packed__)) TLV_t;

static void initialize_nvs()
{
  esp_err_t err = nvs_flash_init();
  if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND)
  {
    ESP_ERROR_CHECK(nvs_flash_erase());
    err = nvs_flash_init();
  }
  ESP_ERROR_CHECK(err);
}

/* Initialize wifi with tcp/ip adapter */
static void initialize_wifi_sniffer()
{
  wifi_promiscuous_filter_t wifi_promiscuous_filter =
  {
    .filter_mask = WIFI_PROMIS_FILTER_MASK_MGMT
  };
  initialize_nvs();
   //tcpip_adapter_init(); //obsolete, remplace par ci dessous
  esp_netif_init();
  ESP_ERROR_CHECK(esp_event_loop_create_default());
  wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
  ESP_ERROR_CHECK(esp_wifi_init(&cfg));
  ESP_ERROR_CHECK(esp_wifi_set_storage(WIFI_STORAGE_RAM));
  ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_NULL));
  //mode long range: ne passe pas en mode sniffeur
  //ESP_ERROR_CHECK(esp_wifi_set_protocol(WIFI_IF_AP, WIFI_PROTOCOL_LR));
  ESP_ERROR_CHECK( esp_wifi_start() );

  esp_wifi_set_promiscuous_filter(&wifi_promiscuous_filter);
  esp_wifi_set_promiscuous_rx_cb(wifi_sniffer_cb);
  ESP_ERROR_CHECK(esp_wifi_set_promiscuous(true));
  esp_wifi_set_channel(wifi_channel, WIFI_SECOND_CHAN_NONE);
  ESP_LOGI(TAG, "start WiFi promiscuous ok");
}

//mise en ram de la fonction callback pour plus de rapidité
IRAM_ATTR static void wifi_sniffer_cb(void *recv_buf, wifi_promiscuous_pkt_type_t type)
{
  static int32_t *oui_ver;
  static int32_t index, ptr, i;
  static int16_t i_value;
  //static uint8_t *hex;
  static int8_t s_value;

  static wifi_promiscuous_pkt_t *sniffer;
  static TLV_t *tlv, *b_tlv;
  static vendor_ie_data_t *gse_vendor;
  static frame_header_t *packet;
  static float f_value;
  static char identifiant[31];

  sniffer = (wifi_promiscuous_pkt_t *)recv_buf;

  if ((*(uint16_t *)sniffer->payload) == 0x0080) //beacon frame
  {
    packet = (frame_header_t*) sniffer->payload;
    index = sizeof(*packet) + packet->ssid_length;
    while (index < (sniffer->rx_ctrl.sig_len - 4))
    {
      tlv = (TLV_t*)((sniffer->payload) + index);
      //information de type Wi-Fi Protected Access/spécific vendor
      if (tlv->type == 0xDD)
      {
        oui_ver = (int32_t*) tlv->payload;
        // est-ce bien notre OUI (specific vendor)
        if ( *oui_ver == OUI_VER)
        {
          gse_vendor = (vendor_ie_data_t*)(tlv);
          ptr = 0;

          //          Serial.print("Longueur de la trame: ");Serial.println(sniffer->rx_ctrl.sig_len - 4);
          //          Serial.println("++++++++++++++++++++++++++++++++++++++");
          //          printf ("\r\n\t ");
          //
          //          hex = (uint8_t*)sniffer->payload;
          //          for (i = 0; i < (sniffer->rx_ctrl.sig_len - 4); i++)
          //          {
          //            // Multiple of 16 means new line (with line offset).
          //
          //            if ((i % 16) == 0)
          //            {
          //              // Don't print ASCII buffer for the "zeroth" line.
          //              if (i != 0) printf ("\r\n\t ");
          //            }
          //
          //            // Now the hex code for the specific character.
          //            printf (" %02X", hex[i]);
          //          }
          //          printf("\r\n");
          //
          //          // il faudrait mettre un controle du numéro de trame par ici
          //          Serial.println("++++++++++++++++++++++++++++++++++++++");
          num_trame = packet->sequence_frag >> 4;
          //          Serial.print("Numéro de trame: ");printf ("%d", num_trame );printf("\r\n");
          //          Serial.println("++++++++++++++++++++++++++++++++++++++");

          while (ptr < (gse_vendor->length - 4))
          {
            b_tlv = (TLV_t*)((gse_vendor->payload) + ptr);
            switch (b_tlv->type)
            {
              case B_VERSION :
                //s_value = (uint8_t)(*(b_tlv->payload));
                break;
              case B_IDENT_FR :
                memcpy (identifiant, b_tlv->payload, b_tlv->length);
                identifiant[b_tlv->length + 1] = '\0';
                // ID balise
                ID_Balise = String(identifiant);
                break;
              case B_IDENT_ANSI :
                //memcpy (identifiant, b_tlv->payload, b_tlv->length);
                //identifiant[b_tlv->length + 1] = '\0';
                break;
              case B_CUR_LAT :
                f_value = (float)((signed)(__bswap_32(*(int32_t*)(b_tlv->payload)))) / 100000.0;
                // Latitude
                LAT_actu = String(f_value, 5);
                break;
              case B_CUR_LON :
                f_value = (float)((signed)(__bswap_32(*(int32_t*)(b_tlv->payload)))) / 100000.0;
                // Longitude
                LON_actu = String(f_value, 5);
                break;
              case B_CUR_ALT :
                i_value = __bswap_16(*(int16_t*)(b_tlv->payload));
                // Altitude
                ALT_actu = String(i_value);
                break;
              case B_CUR_HAU :
                i_value = __bswap_16(*(int16_t*)(b_tlv->payload));
                // hauteur
                HAU_actu = String(i_value);
                break;
              case B_STA_LAT :
                f_value = (float)((signed)(__bswap_32(*(int32_t*)(b_tlv->payload)))) / 100000.0;
                // home latitude
                LAT_home = String(f_value, 5);
                break;
              case B_STA_LON :
                f_value = (float)((signed)(__bswap_32(*(int32_t*)(b_tlv->payload)))) / 100000.0;
                // home longitude
                LON_home = String(f_value, 5);
                break;
              case B_CUR_VIT :
                s_value = *(b_tlv->payload);
                //ground speed
                VIT_actu = String(s_value);
                break;
              case B_CUR_DIR :
                i_value = __bswap_16(*(int16_t*)(b_tlv->payload));
                //direction, cap
                DIR_actu = String(i_value);
                break;
              case B_CODE_INFO :
                //CODEINFO
                codeinfo = (double)(*(b_tlv->payload));;
                break;
              default:
                break;
            }
            ptr = ptr + (b_tlv->length) + 2;
          }

          //on récup les infos wifi
          Wifi_RSSI = String(sniffer->rx_ctrl.rssi);
          Wifi_Bande = String(WIFI_CBW[sniffer->rx_ctrl.cwb]);
          Wifi_Rate = String(WIFI_PHY_RATE[sniffer->rx_ctrl.rate]);
          Wifi_Canal = String(sniffer->rx_ctrl.channel);
          //on indique à la loop que la trame est pleine et qu'il peut l'envoyer au BT
          tramerecue = true;
        } //fin if vendor ok
      } //fin tlv->type 0xDD
      index = index + tlv->length + 2;
    }
  }
}

void setup()
{
  // initialize digital pin LED_BUILTIN as an output.
  pinMode(LED, OUTPUT);

  Serial.begin(115200);
  //led bleue démarrage
  int compteur = 4;
  while (compteur > 0)
  {
    digitalWrite(LED, ON);
    delay(250);
    digitalWrite(LED, OFF);
    delay(250);
    compteur--;
  }
  digitalWrite(LED, OFF);

  //initialisation du BLE
  Serial.println("--------------------------");
  Serial.println("Mise en place du serveur BT");
  InitBT();

  //init wifi
  Serial.println("--------------------------");
  Serial.println("Mise en place du sniffer wifi");
  initialize_wifi_sniffer();
  Serial.println("--------------------------");
  Serial.println("En attente de trames...");
}

uint16_t anciennetrame = 0;
void loop()
{
  //delay(2);
  if (tramerecue == true)
    //c'est ici qu'on peut envoyer la trame au BT
  {
    //préparation de la trame complète:
    //construire la trame dans l'ordre:
    trame = "ID=";
    trame += ID_Balise;
    trame += " lat=";
    trame += LAT_actu;
    trame += " long=";
    trame += LON_actu;
    trame += " alt=";
    trame += ALT_actu;
    trame += " hauteur=";
    trame += HAU_actu;
    trame += " lat_dep=";
    trame += LAT_home;
    trame += " long_dep=";
    trame += LON_home;
    trame += " Vitesse=";
    trame += VIT_actu;
    trame += " Dir=";
    trame += DIR_actu;
    trame += " CodeInfo=";
    trame += String((int)codeinfo);
    //RSSI
    trame += " RSSI=";
    trame += Wifi_RSSI;
    //Largeur de bande
    trame += " Largeur=";
    trame += Wifi_Bande;
    //Vitesse
    trame += " Rate=";
    trame += Wifi_Rate;
    //Canal
    trame += " Canal=";
    trame += Wifi_Canal;
    //Numéro de trame
    trame += " Numero_Trame=";
    trame += String(num_trame);

    //Serial.println("++++++++++++++++++++++++++++++++++++++");
    if (num_trame - anciennetrame > 1)
    {
      Serial.print("écart trame: "); Serial.println(String(num_trame - anciennetrame));
    }
    anciennetrame = num_trame;
#ifdef DEBUG
    Serial.print("Numéro de trame: "); Serial.println(String(num_trame));
    Serial.println("++++++++++++++++++++++++++++++++++++++");
    //temps entre deux trames
    Serial.print("Intervalle: "); Serial.print((millis() - agetrame)); Serial.println(" ms");
    Serial.println("----");
    // visu des infos wifi:
    Serial.print("RSSI="); Serial.println(Wifi_RSSI);
    Serial.print("Largeur="); Serial.println(Wifi_Bande);
    Serial.print("Rate="); Serial.println(Wifi_Rate);
    Serial.print("Canal="); Serial.println(Wifi_Canal);
    //visu de la trame
    Serial.print("Longueur de la trame:"); Serial.println(trame.length());
    Serial.println(trame);
    Serial.println("--------------------------");
#endif
    //c'est ici qu'on envoie la trame au BT
    pCharacteristic->setValue(trame.c_str());
    pCharacteristic->notify();
    //puis on la vide...
    trame = "";
    // et on le dit!
    tramerecue = false;
    // ici réinit le compteur d'age
    agetrame = millis();
    //selon codeinfo on fait varier la vitesse de la led:
    int compteur = 0;
    if (codeinfo == 9)
    {
      //led bleue rythme rock'n'roll, tout est ok
      compteur = 8;
      while (compteur > 0)
      {
        digitalWrite(LED, ON);
        delay(25);
        digitalWrite(LED, OFF);
        delay(25);
        compteur--;
        digitalWrite(LED, ON);
      }
    }//fin if codeinfo 9
    else
    {
      //led bleue rythme slow, souci
      compteur = 2;
      while (compteur > 0)
      {
        digitalWrite(LED, ON);
        delay(100);
        digitalWrite(LED, OFF);
        delay(100);
        compteur--;
        digitalWrite(LED, ON);
      }
    }//fin else codeinfo pas 9
  }
  else //sinon la trame n'est pas vide, on calcule depuis combien de temps...
  {
    if (millis() > (agetrame + 6000))
    {
      if (millis() > (comptesecondes + 1000))
      {
        //Serial.print("plus de trame reçue depuis + de  "); Serial.print((millis() - agetrame) / 1000); Serial.println(" s");
        comptesecondes = millis();
        if (digitalRead(LED) == OFF)
        {
          digitalWrite(LED, ON);
        }
        else
        {
          digitalWrite(LED, OFF);
        }
      }
    }
  }//fin de trame pas vide
}
