#include <esp_now.h>

#include "WiFi.h"

//-------------------------------------------

//            Control de variables

//-------------------------------------------

double anglex;

double angley;

double dx;

double dy;

double mESq_brut;

double mDret_brut;

double me;

double md;

double m3;

String s_dades;

boolean b_L1;

boolean b_R1;

uint8_t espnow_peerMAC[] = {0x3C, 0xE9, 0x0E, 0x6C, 0x49, 0x44};

String espnow_rx_data="";

String espnow_rx_mac="";

esp_err_t espnow_send_result;

esp_now_peer_info_t espnow_peerInfo;

QueueHandle_t espnow_thread_rx_queue;

typedef struct T_espnow_thread_queue_data{ char *data; char *mac;};

T_espnow_thread_queue_data *espnow_thread_queue_data;

bool espnow_encrypted=false;

static const char* ESPNOW_PMK_KEY_STR = "";

static const char* ESPNOW_LMK_KEY_STR  = "";

//--------------------------------------------------------

//              Control de funcions

//--------------------------------------------------------

String fnc_json_encodeNum(double _v){

    if(isnan(_v))return "0";

    String t=String(_v,6);

    int lasti=t.length()-1;

    while(t[lasti]=='0')lasti--;

    if(t[lasti]=='.')lasti--;

    return t.substring(0,lasti+1);

}

void espnow_send_str(String _mac, String _data) {

  uint8_t m1,m2,m3,m4,m5,m6;

    sscanf(_mac.c_str(),"%2x:%2x:%2x:%2x:%2x:%2x",&m1,&m2,&m3,&m4,&m5,&m6);

  espnow_peerMAC[0]=m1;

  espnow_peerMAC[1]=m2;

  espnow_peerMAC[2]=m3;

  espnow_peerMAC[3]=m4;

  espnow_peerMAC[4]=m5;

  espnow_peerMAC[5]=m6;

    espnow_send_result=esp_now_send(espnow_peerMAC, (uint8_t *) _data.c_str(), _data.length());

}

void espnow_task_queue_rx(void *parameter){

  for(;;){

      T_espnow_thread_queue_data *rxData;

      xQueueReceive(espnow_thread_rx_queue, &rxData, portMAX_DELAY);

    espnow_rx_data=String(rxData->data);

    espnow_rx_mac=String(rxData->mac);

    espnow_on_data_received();

    free(rxData->data);

    free(rxData->mac);

  }

}

void espnow_data_rx(const uint8_t * _mac, const uint8_t *_incomingData, int _len) {

  //remote MAC

  espnow_rx_mac="";

    for (byte i = 0; i < 6; ++i)

    {

      char buf[3];

      sprintf(buf, "%2X", _mac[i]);

      espnow_rx_mac += buf;

      if (i < 5) espnow_rx_mac += ':';

    }

  //payload DATA

  char* buff = (char*) _incomingData;

    buff[_len]=NULL;

  //data packet to rx queue

    espnow_thread_queue_data=(T_espnow_thread_queue_data*) malloc(sizeof(T_espnow_thread_queue_data*));

    espnow_thread_queue_data->data = (char*) malloc(_len+1);

    espnow_thread_queue_data->mac = (char*) malloc(espnow_rx_mac.length()+1);

    strcpy(espnow_thread_queue_data->data, buff);

    strcpy(espnow_thread_queue_data->mac, espnow_rx_mac.c_str());

  xQueueSendToBack(espnow_thread_rx_queue, &espnow_thread_queue_data, portMAX_DELAY);

}

void espnow_on_data_received() {

}

void espnow_add_peer_str(String _mac) {

  uint8_t m1,m2,m3,m4,m5,m6;

    sscanf(_mac.c_str(),"%2x:%2x:%2x:%2x:%2x:%2x",&m1,&m2,&m3,&m4,&m5,&m6);

  espnow_peerMAC[0]=m1;

  espnow_peerMAC[1]=m2;

  espnow_peerMAC[2]=m3;

  espnow_peerMAC[3]=m4;

  espnow_peerMAC[4]=m5;

  espnow_peerMAC[5]=m6;

  memcpy(espnow_peerInfo.peer_addr, espnow_peerMAC, 6);

  espnow_peerInfo.channel = 0;

  espnow_peerInfo.encrypt = espnow_encrypted;

  if(espnow_encrypted){

    for (uint8_t i = 0; i < 16; i++) {

        espnow_peerInfo.lmk[i] = ESPNOW_LMK_KEY_STR[i];

      }

  }

  esp_now_add_peer(&espnow_peerInfo);

}

//---------------------------------------

//          Preparació

//--------------------------------------

void setup()

{

  pinMode(35, INPUT);

  pinMode(34, INPUT);

  pinMode(23, INPUT);

  pinMode(5, INPUT);

  Serial.begin(115200);

  Serial.flush();

  while(Serial.available()>0)Serial.read();

  WiFi.mode(WIFI_STA);

  if (esp_now_init() != ESP_OK){

    delay(1000);

    ESP.restart();

  }

  espnow_thread_rx_queue = xQueueCreate(4, sizeof(T_espnow_thread_queue_data*));

  xTaskCreatePinnedToCore(espnow_task_queue_rx,"T_espnow_rx",2048,NULL,10,NULL,1);

  esp_now_register_recv_cb(esp_now_recv_cb_t(espnow_data_rx));

  espnow_add_peer_str(String("3C:E9:0E:6C:44"));

}

//----------------------------------------------------

//              Recollida de dades

//----------------------------------------------------

void loop()

{

  yield();

    anglex = map(analogRead(35), 0,4095,0,255);

    angley = map(analogRead(34), 0,4095,0,255);

    Serial.println(String("angleX ")+String(anglex));

    Serial.println(String("angleY ")+String(angley));

    dx = (anglex - 128);

    dy = (128 - angley);

    Serial.println(String(" DY ")+String(dy));

    Serial.println(String("DX ")+String(dx));

    Serial.println(String("mESQ ")+String(mESq_brut));

    Serial.println(String("mDRET ")+String(mDret_brut));

    if ((abs(dx) < 10)) {

      dx = 0;

    }

    if ((abs(dy) < 10)) {

      dy = 0;

    }

    me = (dy + dx);

    md = (dy - dx);

    if ((me > 127)) {

      me = 127;

    }

    if ((me < -127)) {

      me = -127;

    }

    if ((md > 127)) {

      md = 127;

    }

    if ((md < -127)) {

      md = -127;

    }

    b_L1 = (!digitalRead(23));

    b_R1 = (!digitalRead(5));

    m3 = 0;

    if ((b_L1 && (!b_R1))) {

      m3 = 1;

    }

    else if ((b_R1 && (!b_L1))) {

      m3 = -1;

    }

    s_dades = String("{")+String((String("\"")+String("me")+String("\"")+String(":")+fnc_json_encodeNum(me)))+String(",")+String((String("\"")+String("md")+String("\"")+String(":")+fnc_json_encodeNum(md)))+String(",")+String((String("\"")+String("m3")+String("\"")+String(":")+fnc_json_encodeNum(m3)))+String("}");

    espnow_send_str(String("3C:E9:0E:6C:44"),String(s_dades));

    delay(10);

}