/* Basic Multi Threading Arduino 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.
*/
// Please read file README.md in

#include <freertos/FreeRTOS.h>
#include <freertos/queue.h>
#include <string.h>
#include <ctime>
#include <iostream>
#include <unistd.h>

static const char *const TAG = "espnow";
#define MAX_LINE_LENGTH (64)
static uint16_t last_ref_id = 0;
// Define two tasks for reading and writing from and to the serial port.
TaskHandle_t my_task_handler{nullptr};
void my_task(void *pvParameters);

// Define Queue handle
QueueHandle_t QueueHandle;
const int QueueElementSize = 10;
typedef struct {
  char line[MAX_LINE_LENGTH];
  uint16_t ref_id;
  uint8_t retrys : 3;

  void retry() {
    if (this->retrys < 7 ) {
      retrys = retrys + 1;
    }
  }
} message_t;



std::string gen_random(const int len) {
    static const char alphanum[] =
        "0123456789 "
        "ABCDEFG-HIJKLMNOP_QRSTUVWXYZ="
        "abcde,fghijklmn.opqrst;uvwxyz:";
    std::string tmp_s;
    tmp_s.reserve(len);

    for (int i = 0; i < len; ++i) {
        tmp_s += alphanum[rand() % (sizeof(alphanum) - 1)];
    }
    
    return tmp_s;
}

void add_message() {
  message_t message;
  std::string line = gen_random(rand() % (sizeof(message.line)-1));
  memcpy(&message.line,line.c_str() , line.size() );
  message.ref_id = ++last_ref_id;
  message.retrys = 0;
  // The line needs to be passed as pointer to void.
  // The last parameter states how many milliseconds should wait (keep trying to send) if is not possible to send right away.
  // When the wait parameter is 0 it will not wait and if the send is not possible the function will return errQUEUE_FULL
  int ret = xQueueSend(QueueHandle, (void *)&message, 0);
  if (ret == pdTRUE) {
    // The message was successfully sent.
  } else if (ret == errQUEUE_FULL) {
    // Since we are checking uxQueueSpacesAvailable this should not occur, however if more than one task should
    //   write into the same queue it can fill-up between the test and actual send attempt
    Serial.println("The `TaskReadFromSerial` was unable to send data into the Queue");
  }  // Queue send check

}



// The setup function runs once when you press reset or power on the board.
void setup() {
  // Initialize serial communication at 115200 bits per second:
  Serial.begin(115200);
  while (!Serial) {
    delay(10);
  }
  pinMode(18, OUTPUT);
  // Create the queue which will have <QueueElementSize> number of elements, each of size `message_t` and pass the address to <QueueHandle>.
  QueueHandle = xQueueCreate(QueueElementSize, sizeof(message_t));
  if (QueueHandle == NULL) {
    Serial.println("Queue could not be created. Halt.");
    while (1) {
      delay(1000);  // Halt at this point as is not possible to continue
    }
  }
  add_message();
  add_message();

  // Set up two tasks to run independently.
  xTaskCreate(
    my_task, "Task Write To Serial"  // A name just for humans
    ,
    2048  // The stack size can be checked by calling `uxHighWaterMark = uxTaskGetStackHighWaterMark(NULL);`
    ,
    NULL  // No parameter is used
    ,
    2  // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
    ,
    &my_task_handler  // Task handle is not used here
  );


  // Now the task scheduler, which takes over control of scheduling individual tasks, is automatically started.
  Serial.printf(
    "\n queue test.\n\n",
    MAX_LINE_LENGTH - 1
  );
}

void loop() {
  // Loop is free to do any other work

  delay(1000);  // While not being used yield the CPU to other tasks
  xTaskNotifyAndQuery(my_task_handler, 0, eSetValueWithOverwrite, NULL );
 
 // digitalWrite(18, LOW);   // turn the LED on (HIGH is the voltage level)

}

/*--------------------------------------------------*/
/*---------------------- Tasks ---------------------*/
/*--------------------------------------------------*/

void my_task(void *pvParameters) {  // This is a task.
  message_t packet;
  uint32_t state = 0;
  xTaskNotifyAndQuery(my_task_handler, 0, eSetValueWithOverwrite, NULL );
 
  for (;;) {
    xTaskNotifyAndQuery(my_task_handler, 0, eNoAction, &state);
    digitalWrite(18, state==1);    // turn the LED off by making the voltage LOW

    if (state == 1) {
      vTaskDelay( 10 / portTICK_PERIOD_MS );
    } else if (xQueueReceive(QueueHandle, &packet, 10 / portTICK_PERIOD_MS) == pdTRUE ) {
      Serial.println( "packet exist");
      packet.retry();
      if (packet.retrys == 6) {
        Serial.println( "To many send retries. Packet dropped.");
      } else {
        //packet.timestamp = std::time(nullptr);
        xQueueSendToFront(QueueHandle, &packet, 10 / portTICK_PERIOD_MS);
        xTaskNotifyAndQuery(my_task_handler, 1, eSetValueWithOverwrite, NULL );
        Serial.println( packet.line);
        esp_err_t err = ESP_OK; // esp_now_send((uint8_t *) packet.mac, packet.data, packet.size);

        //digitalWrite(18, LOW);    // turn the LED off by making the voltage LOW

        if (err != ESP_OK) {
          //          ESP_LOGI(TAG, "Packet send failed (%d) Error: %s", packet.retrys, esp_err_to_name(err));
          //xTaskNotifyStateClear(my_task_handler);
        } else {
          Serial.printf( "Send Packet (%d). Wait for conformation.\n", packet.retrys);
          //    set_timeout("espnow_send_timeout", 10, [task]() {
          //     xTaskNotifyStateClear(task);
          //   });
        }
      }
    }
  }
}