/*

Florenc Caminade
Thomas FLayols
Etienne Arlaud
Jurij Podgoršek

Receive raw 802.11 packet and filter ESP-NOW vendor specific action frame using BPF filters.
https://hackaday.io/project/161896
https://github.com/thomasfla/Linux-ESPNOW

Adapted from :
https://stackoverflow.com/questions/10824827/raw-sockets-communication-over-wifi-receiver-not-able-to-receive-packets

1/Find your wifi interface:
$ iwconfig

2/Setup your interface in monitor mode :
$ sudo ifconfig wlp5s0 down
$ sudo iwconfig wlp5s0 mode monitor
$ sudo ifconfig wlp5s0 up

3/Run this code as root or regular user!
*/
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <linux/if_arp.h>
#include <arpa/inet.h>
#include <assert.h>
#include <linux/filter.h>

// Debug?
//#define DEBUG

// Booleans
#include <stdbool.h>

// OSC
#include "lo/lo.h"
#include "lo/lo_lowlevel.h"
#include "lo/lo_osc_types.h"

// OSC destination; localhost supercollider running @ default port (57121)
// @TODO get this from argv?
lo_address osc_dest;

 //Approximate
#define PACKET_LENGTH 400
#define MAX_PACKET_LEN 1000

// Maksimalno stevilo
#define ST_SPREJEMNIKOV 10

// Receiver MAC start at byte 52
#define WLAN_DA_OFFSET 52
/*our MAC address*/
uint8_t sprejemnikMac[] = { 0x08, 0x3A, 0xF2, 0x50, 0xEF, 0x6C };
uint8_t wlan_da[] = { 0x0, 0x0, 0x0, 0x0, 0x0, 0x0 };

// ESPNOW packet identifier
#define ESP_ID_OFFSET 82
uint8_t esp_id[] = { 0x18, 0xfe, 0x34, 0x04 };
uint8_t pkg_header[] = { 0x0, 0x0, 0x0, 0x0 };

// ESPNOW data payload starts at byte 87
#define ESP_DATA_OFFSET 87

// Sensor message
#include "../src/sensor_msg.h"

uint8_t odcitekId;
sensor_msg  odcitki[ST_SPREJEMNIKOV];
bool poslji[ST_SPREJEMNIKOV];

#include <sys/time.h>
struct timeval cas;
struct timeval zdaj;
int eps = 0;

/*ESP8266 host MAC address*/
//{0x84,0xF3,0xEB,0x73,0x55,0x0D};


//filter action frame packets
  //Equivalent for tcp dump :
    //type 0 subtype 0xd0 and wlan[24:4]=0x7f18fe34 and wlan[32]=221 and wlan[33:4]&0xffffff = 0x18fe34 and wlan[37]=0x4
//NB : There is no filter on source or destination addresses, so this code will 'receive' the action frames sent by this computer...
#define FILTER_LENGTH 20
static struct sock_filter bpfcode[FILTER_LENGTH] = {
  { 0x30, 0, 0, 0x00000003 },	// ldb [3]	// radiotap header length : MS byte
  { 0x64, 0, 0, 0x00000008 },	// lsh #8	// left shift it
  { 0x7, 0, 0, 0x00000000 },	// tax		// 'store' it in X register
  { 0x30, 0, 0, 0x00000002 },	// ldb [2]	// radiotap header length : LS byte
  { 0x4c, 0, 0, 0x00000000 },	// or  x	// combine A & X to get radiotap header length in A
  { 0x7, 0, 0, 0x00000000 },	// tax		// 'store' it in X
  { 0x50, 0, 0, 0x00000000 },	// ldb [x + 0]		// right after radiotap header is the type and subtype
  { 0x54, 0, 0, 0x000000fc },	// and #0xfc		// mask the interesting bits, a.k.a 0b1111 1100
  { 0x15, 0, 10, 0x000000d0 },	// jeq #0xd0 jt 9 jf 19	// compare the types (0) and subtypes (0xd)
  { 0x40, 0, 0, 0x00000018 },	// Ld  [x + 24]			// 24 bytes after radiotap header is the end of MAC header, so it is category and OUI (for action frame layer)
  { 0x15, 0, 8, 0x7f18fe34 },	// jeq #0x7f18fe34 jt 11 jf 19	// Compare with category = 127 (Vendor specific) and OUI 18:fe:34
  { 0x50, 0, 0, 0x00000020 },	// ldb [x + 32]				// Begining of Vendor specific content + 4 ?random? bytes : element id
  { 0x15, 0, 6, 0x000000dd },	// jeq #0xdd jt 13 jf 19		// element id should be 221 (according to the doc)
  { 0x40, 0, 0, 0x00000021 },	// Ld  [x + 33]				// OUI (again!) on 3 LS bytes
  { 0x54, 0, 0, 0x00ffffff },	// and #0xffffff			// Mask the 3 LS bytes
  { 0x15, 0, 3, 0x0018fe34 },	// jeq #0x18fe34 jt 16 jf 19		// Compare with OUI 18:fe:34
  { 0x50, 0, 0, 0x00000025 },	// ldb [x + 37]				// Type
  { 0x15, 0, 1, 0x00000004 },	// jeq #0x4 jt 18 jf 19			// Compare type with type 0x4 (corresponding to ESP_NOW)
  { 0x6, 0, 0, 0x00040000 },	// ret #262144	// return 'True'
  { 0x6, 0, 0, 0x00000000 },	// ret #0	// return 'False'
};

void print_packet(uint8_t *data, int len) {
  // Beri samo primerne pakete!
  memcpy(&wlan_da, data + WLAN_DA_OFFSET, 6);
#ifdef DEBUG
  printf("Dest MAC: %02x:%02x:%02x:%02x:%02x:%02x\n", wlan_da[0],wlan_da[1],wlan_da[2],wlan_da[3],wlan_da[4],wlan_da[5]);
#endif

  /* DEBUG - print whole raw packet */
#ifdef DEBUG
  for (int i = 0; i < len; i++) {
    printf("0x%02x ", data[i]);
  }
  printf("\n");
#endif

  // Ignoriraj pakete, ki so namenjeni drugam
  for (int i = 0; i < 6; i++) {
    if (wlan_da[i] != sprejemnikMac[i]) {
      return;
    }
  }

  // Ignoriraj pakete, ki niso ESP paketi!
  memcpy(&pkg_header, data + ESP_ID_OFFSET, 4);
  for (int i = 0; i < 4; i++) {
    if (pkg_header[i] != esp_id[i]) {
      return;
    } 
  }

  // Stetje paketov na sekundo
  eps += 1;
  gettimeofday(&zdaj, NULL);

  if (zdaj.tv_sec != cas.tv_sec) {
    printf("Paketov na sekundo: %i\n", eps);
    eps = 0;
    gettimeofday(&cas, NULL); 
  }


  // ID senzorja
  memcpy(&odcitekId, data + ESP_DATA_OFFSET, 1);

#ifdef DEBUG
  printf("Odcitek ID: %i\n", odcitekId); 
#endif

  // Vrednosti
  memcpy(&odcitki[odcitekId], data + ESP_DATA_OFFSET, sizeof(sensor_msg));
  poslji[odcitekId] = true;

#ifdef DEBUG
  printf("senzor %i\n", odcitekId);
  printf("aX: %f \n", odcitki[odcitekId].aX);
  printf("aY: %f \n", odcitki[odcitekId].aY);
  printf("aZ: %f \n", odcitki[odcitekId].aZ);
  printf("qW: %f \n", odcitki[odcitekId].qW);
  printf("qX: %f \n", odcitki[odcitekId].qX);
  printf("qY: %f \n", odcitki[odcitekId].qY);
  printf("qZ: %f \n", odcitki[odcitekId].qZ);
  printf("bat: %f \n", odcitki[odcitekId].bat);
#endif


  // @TODO locen thread za posiljanje?
  char glava[32];
  lo_bundle svezenj;
  lo_message m;
  size_t dolzina;

  for (int i = 0; i < ST_SPREJEMNIKOV; i++) {
    if (poslji[i]) {
      // Ustvarim bundle (svezenj)
      svezenj = lo_bundle_new(LO_TT_IMMEDIATE);

      // Dodamo sporocila
      sprintf(glava, "/ww/%d/acc", i); 
      m = lo_message_new();
      lo_message_add_float(m, odcitki[i].aX);
      lo_message_add_float(m, odcitki[i].aY);
      lo_message_add_float(m, odcitki[i].aZ);

      lo_bundle_add_message(svezenj, glava, m);
      sprintf(glava, "/ww/%d/quat", i); 
      m = lo_message_new();
      lo_message_add_float(m, odcitki[i].qW);
      lo_message_add_float(m, odcitki[i].aX);
      lo_message_add_float(m, odcitki[i].aY);
      lo_message_add_float(m, odcitki[i].aZ);

      lo_bundle_add_message(svezenj, glava, m);

#ifdef DEBUG
      lo_bundle_pp(svezenj);
#endif

      lo_send_bundle(osc_dest, svezenj);
      lo_bundle_free(svezenj);
    }
  }
}

int create_raw_socket(char *dev, struct sock_fprog *bpf)
{
    struct sockaddr_ll sll;
    struct ifreq ifr;
    int fd, ifi, rb, attach_filter;

    bzero(&sll, sizeof(sll));
    bzero(&ifr, sizeof(ifr));

    fd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
    assert(fd != -1);

    strncpy((char *)ifr.ifr_name, dev, IFNAMSIZ);
    ifi = ioctl(fd, SIOCGIFINDEX, &ifr);
    assert(ifi != -1);

    sll.sll_protocol = htons(ETH_P_ALL);
    sll.sll_family = PF_PACKET;
    sll.sll_ifindex = ifr.ifr_ifindex;
    sll.sll_pkttype = PACKET_OTHERHOST;

    rb = bind(fd, (struct sockaddr *)&sll, sizeof(sll));
    assert(rb != -1);

    attach_filter = setsockopt(fd, SOL_SOCKET, SO_ATTACH_FILTER, bpf, sizeof(*bpf));
    assert(attach_filter != -1);

    return fd;
}

int main(int argc, char **argv) {
  assert(argc == 3);

  uint8_t buff[MAX_PACKET_LEN] = {0};
  int sock_fd;
  char *dev = argv[1];
  char *scport = argv[2];
  struct sock_fprog bpf = {FILTER_LENGTH, bpfcode};

  sock_fd = create_raw_socket(dev, &bpf); /* Creating the raw socket */

  osc_dest = lo_address_new("localhost", scport);

  printf("\n Waiting to receive packets ........ \n");
  gettimeofday(&cas, NULL);

  while (1) {
    int len = recvfrom(sock_fd, buff, MAX_PACKET_LEN, MSG_TRUNC, NULL, 0);

    if (len < 0) {
      perror("Socket receive failed or error");
      break;
    } else {
      //printf("len:%d\n", len);
      print_packet(buff, len);
    }
  }
  close(sock_fd);
  return 0;
}