#include <Arduino_FreeRTOS.h>
#include <task.h>
#include <semphr.h>  // add the FreeRTOS functions for Semaphores (or Flags).
#include <FreeRTOSConfig.h>


/* Define the traceTASK_SWITCHED_IN() and *_OUT() macros become HIGH when the tasks is switched in, and low w
when switched out. This has to be done in the RTOS Library. */

//#define traceTASK_SWITCHED_IN() digitalWrite( (int)pxCurrentTCB->pxTaskTag, HIGH )
//#define traceTASK_SWITCHED_OUT() digitalWrite((int)pxCurrentTCB->pxTaskTag, LOW )

/* Define the pin on which the tasks is being monitored (to be put in each task) */ /*modified by Markus Pfeil*/
/* vTaskSetApplicationTaskTag( NULL, ( void * )data.debug_pin); */ 

SemaphoreHandle_t SimpleMutex;
SemaphoreHandle_t BinSemaphore;

TaskHandle_t HPT_Handler;
TaskHandle_t MPT_Handler;
TaskHandle_t LPT_Handler;

void HPT_Task (void *argument);
void MPT_Task (void *argument);
void LPT_Task (void *argument);


void Send_Uart (String str)
{
  xSemaphoreTake(BinSemaphore /*SimpleMutex*/, portMAX_DELAY);
  delay(10);
  Serial.println(str);
  //delay(100);
  xSemaphoreGive(BinSemaphore /*SimpleMutex*/);
}

void HPT_Task (void *argument)
{
  vTaskSetApplicationTaskTag( NULL, ( void * )13);

  String strtosend = "IN HPT===========================I\n";
  while (1)
  {
    xSemaphoreTake(BinSemaphore /*SimpleMutex*/, portMAX_DELAY);
    Serial.println("Entered HPT and About to take MUTEX\n");
    //delay(100);
    xSemaphoreGive(BinSemaphore /*SimpleMutex*/);
    Send_Uart(strtosend);
    
     xSemaphoreTake(BinSemaphore /*SimpleMutex*/, portMAX_DELAY);
    Serial.println("Leaving HPT\n\n");
    //delay(100);
    xSemaphoreGive(BinSemaphore /*SimpleMutex*/);
    vTaskDelay(pdMS_TO_TICKS(150));
  }
}

void MPT_Task (void *argument)
{
  vTaskSetApplicationTaskTag( NULL, ( void * )12);
  String strtosend = "IN MPT...........................I\n";
  while (1)
  {
    //Serial.println(strtosend);
    Send_Uart(strtosend);
    //vTaskDelay(pdMS_TO_TICKS(150));
  }
}

void LPT_Task (void *argument)
{
  vTaskSetApplicationTaskTag( NULL, ( void * )11);
  String strtosend = "IN LPT===========================I\n";
  while (1)
  {

    Serial.println("Entered LPT and About to take MUTEX\n");
    Send_Uart(strtosend);
    Serial.println("Leaving LPT\n\n");
    //delay(2000);
    vTaskDelay(pdMS_TO_TICKS(150));
  }
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(13, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(11, OUTPUT);

 SimpleMutex = xSemaphoreCreateMutex();

  if (SimpleMutex != NULL)
  {
	  Serial.println("Mutex Created\n\n");
  }

  BinSemaphore = xSemaphoreCreateBinary();
  if (BinSemaphore != NULL)
  {
	  Serial.println("Semaphore Created\n\n");
  }

  xSemaphoreGive(BinSemaphore);

  /// create tasks

  xTaskCreate(HPT_Task, "HPT", 128, NULL, 3, &HPT_Handler);
  xTaskCreate(MPT_Task, "MPT", 128, NULL, 2, &MPT_Handler);
  xTaskCreate(LPT_Task, "LPT", 128, NULL, 1, &LPT_Handler);

  vTaskStartScheduler();
}

void loop() {
  //nothing here

}