/*
  ESP32 multitasking
*/
#define CORE_0  0
#define CORE_1  1
TaskHandle_t Task1;
TaskHandle_t Task2;

const int led_1 = 32;
const int led_2 = 25;

char queue[20][150];
int front = -1, rear = -1;


void setup() {
  Serial.begin(115200); 
  pinMode(led_1, OUTPUT);
  pinMode(led_2, OUTPUT);

  xTaskCreatePinnedToCore(  SerialTask, //task name
                            "SerialTask", //text
                             10000, //task size
                             NULL,  
                             1,     
                             &Task1,  //handler
                             CORE_0 );  //core id

  xTaskCreatePinnedToCore(  Task2code,
                            "Task2",
                            1024,
                            NULL,
                            2,
                            &Task2,
                            CORE_1  );          
  delay(500); 
}

void SerialTask( void * parameter ){
  Serial.print("Task1 is running on core ");
  Serial.println(xPortGetCoreID());
  char received_character = NULL;
  char uart_command_buffer[30];
  int uart_command_buffer_index = 0;
  bool data_reception = false;
  

  for(;;){

    if(Serial.available()){
      received_character = Serial.read();
      if(received_character == 'C' || data_reception == true){
        if(received_character == 'F'){
          uart_command_buffer[uart_command_buffer_index] = received_character;
          /*Queue implementation to store commands*/
          for(int queue_rear_index = 1, uart_command_index = 0; uart_command_index < (uart_command_buffer_index+1); queue_rear_index++, uart_command_index++){
              queue[rear][queue_rear_index] = uart_command_buffer[uart_command_index];
              // Serial.print(queue[rear][queue_rear_index]);
          }
          Serial.println("Inserted");
          rear++; //Increment the queue rear by one
          data_reception = false;
          uart_command_buffer_index = 0;
        }
        else{
            data_reception = true;
            uart_command_buffer[uart_command_buffer_index] = received_character;
            uart_command_buffer_index++;
        }
      }
    }
  }
}

void Task2code( void * parameter ){
  bool queue_popped = false;
  char mqtt_queue_buffer[150];
  unsigned long queueTaskCurrentMillis = 0, queueCommandPoppedMillis = 0;
  
  for(;;){
    queueTaskCurrentMillis = millis();
    if(front != rear){
      if(queue_popped){
        if( queueTaskCurrentMillis - queueCommandPoppedMillis > 2000){
          queue_popped = false;
          queueCommandPoppedMillis = queueTaskCurrentMillis;
        }
      }
      
      if(queue_popped == false){
        int QUEUE_INDEX = 1;
        while(queue[front][QUEUE_INDEX] != '\0' && QUEUE_INDEX < 1500){
          mqtt_queue_buffer[QUEUE_INDEX - 1] = queue[front][QUEUE_INDEX];
          QUEUE_INDEX++;
        }
        queue_popped = true;
        front++;
        Serial.println(mqtt_queue_buffer);
        queueCommandPoppedMillis = millis();
      }
    }
    else if (front == rear) {
      front = 1;
      rear = 1;
    }
    Serial.print("Front: ");
    Serial.println(front);
    Serial.print("Rear: ");
    Serial.println(rear);
    delay(1000);
  }
}

void loop() {
  
}