#include <Arduino_FreeRTOS.h>
#include <Streaming.h>

#define BAUD_RATE 9600

#define SendTo_MicroControlle_LED_pin 8
#define SendTo_PC_pin 9
#define PID_control_pin 10
#define Analog_Read_LED_pin 11
#define TaskIDLE_LED_pin 12
#define ananlog_read_position_sensor A0

#define SendTo_MicroController_stack_size 120
#define SendTo_PC_stack_size 120
#define PID_control_stack_size 120
#define Analog_Read_stack_size 120
#define Idling_free_task_stack_size 100

enum TASK_STATE {NOT_ASSIGNED, RUNNING, BLOCKED, READY, SUSPENDED};

// enum TASK_STATE task1_STATE = NOT_ASSIGNED;
// enum TASK_STATE task2_STATE = NOT_ASSIGNED;
// enum TASK_STATE task3_STATE = NOT_ASSIGNED;
// enum TASK_STATE task4_STATE = NOT_ASSIGNED;
// enum TASK_STATE taskIDLE_STATE = NOT_ASSIGNED;

int throttle_pos = 0;

// static unsigned long task1_begin_time = 0;
// static unsigned long task1_end_time = 0;
// static unsigned long task2_begin_time = 0;
// static unsigned long task2_end_time = 0;
// static unsigned long task3_begin_time = 0;
// static unsigned long task3_end_time = 0;
// static unsigned long task4_begin_time = 0;
// static unsigned long task4_end_time = 0;

static void SendTo_MicroController(void* pvParameters);
static void SendTo_PC(void* pvParameters);
static void PID_control(void* pvParameters);
static void Analog_Read(void* pvParameters);
static void Idling_free_task(void* pvParameters);
void setup(){
  Serial.begin( BAUD_RATE );
  Serial << "In Setup function\n";
  //Đặt các chân 8 - 12 ở chế độ xuất
  pinMode(SendTo_MicroControlle_LED_pin, OUTPUT);
  pinMode(SendTo_PC, OUTPUT);
  pinMode(PID_control, OUTPUT);
  pinMode(Analog_Read_LED_pin, OUTPUT);
  pinMode(TaskIDLE_LED_pin, OUTPUT);


  //SendTo_MicroController: tần số = 20 Hz (trong 1 giây gửi thông tin mới tới Vi xử lý 20 lần)
  xTaskCreate(SendTo_MicroController,  //Hàm task gọi
              "Task1",   //Tên task để người lập trình dễ theo dõi
              SendTo_MicroController_stack_size,       //Số lượng từ trong bộ nhớ stack được cấp phát cho task (đối với Arduino Uno..., con số này không được vượt quá 130)
              NULL,      //Giá trị tham số truyền cho task, không thể truyền địa chỉ của biến trong stack cho task
              1,         //Mức ưu tiên của task, ưu tiên càng cao càng task càng được ưu tiên sử dụng tài nguyên
              NULL);     //Task_handle, là tên task dùng để truyền cho các hàm API để quản lý các tasks
  
  //SendTo_PC: Gửi thông tin từ màn hình Serial đến PC, tần số = 500 Hz
  xTaskCreate(SendTo_PC, "Task2", SendTo_PC_stack_size, NULL, 2, NULL);
  
  //PID_control: Điều khiển vị trí bướm ga PID, tần số = 50 Hz (1 giây điều chỉnh vị trí 50 lần)
  xTaskCreate(PID_control, "Task3", PID_control_stack_size, NULL, 3, NULL);
  
  //Analog_Read: Đọc giá trị tín hiệu Analog truyền từ cảm biến vị trí bướm ga đến Vi xử lý
  //tần số = 1000 Hz; Đọc 1000 lần mỗi giây, cứ mỗi lần đọc được 100 giá trị thì thì lấy trung bình rồi gửi vô buffer chung với PID_control
  xTaskCreate(Analog_Read, "Task4", Analog_Read_stack_size, NULL, 2, NULL);
  
  //Idling_free_task: tác vụ nhàn rỗi, không làm gì, mức ưu tiên thấp nhất, chỉ chạy khi không có tác vụ nào khác READY hoặc RUNNING
  xTaskCreate(Idling_free_task, "IdleTask", 100, NULL, 0, NULL);
}

void loop(){
  //There is no instruction in the loop section of the code.
  // Because each task executes on interrupt after specified time
}

//The following function is Task1. We display the task label on Serial monitor.

static void SendTo_MicroController(void* pvParameters) {
 int SendTo_MicroControlle_LED_state  = 0;
 int SendTo_MicroController_counter = 0; 
 int f1 = 220; // Tần số của tác vụ gửi thông tin đến Vi xử lý
  while (1){
    SendTo_MicroController_counter++;
    throttle_pos++;
    
    unsigned long SendTo_MicroController_time_begin = micros();
      
    //Thực hiện công việc chính của task: gửi dữ liệu từ máy tính tới Vi xử lý
    for (SendTo_MicroController_counter; SendTo_MicroController_counter++ >= f1; SendTo_MicroController_counter++)
    {
       Serial << "Vi tri buom ga (%): " << throttle_pos << " counter: " << SendTo_MicroController_counter << "\n";
    
  //Bật tắt một LED để dễ kiểm tra tính thời gian thực của tác vụ, chân 8 của Arduino 
    SendTo_MicroControlle_LED_state = !SendTo_MicroControlle_LED_state;
    digitalWrite(SendTo_MicroControlle_LED_pin, SendTo_MicroControlle_LED_state);
  //Cập nhậtS
    throttle_pos++;
    throttle_pos = throttle_pos * 2;
    if ( throttle_pos > 50 ) throttle_pos = 0;
    }
    
    //Kết thúc công việc trong 1 chu kỳ (đã gửi dữ liệu từ PC tới Vi xử lý 20 lần)
    unsigned long  SendTo_MicroController_time_end = micros();
    
    //Tính toán thời gian delay cho task
    unsigned long SendTo_MicroController_time_left_over = SendTo_MicroController_time_end - SendTo_MicroController_time_begin;
    if ( SendTo_MicroController_time_left_over / 1000 >= 0 ) {
      vTaskDelay( (SendTo_MicroController_time_left_over / 1000) / portTICK_PERIOD_MS);
    }
  }
  vTaskDelete(NULL);
}

static void SendTo_PC(void* pvParameters) {
  long number_task2 = 0;
  int SendTo_PC_LED_state = 0;
  int SendTo_PC_counter = 0;
  while (1) {
    Serial << "\n";
    SendTo_PC_counter++;
   
    unsigned long SendTo_PC_time_begin = micros();
    for (SendTo_PC_counter ; SendTo_PC_counter <= 100; SendTo_PC_counter++) {
      Serial << "SendTo_PC\n";
      number_task2++;
      SendTo_PC_LED_state = !SendTo_PC_LED_state;
    digitalWrite(SendTo_PC_pin, SendTo_PC_LED_state);
    }
    unsigned long SendTo_PC_time_end = micros();
    
    //Tính toán thời gian delay cho task
    unsigned long SendTo_PC_time_left_over = SendTo_PC_time_end - SendTo_PC_time_begin;
    if ( SendTo_PC_time_left_over / 1000 >= 0 ) {
      vTaskDelay( (SendTo_PC_time_left_over / 1000) / portTICK_PERIOD_MS);
    }
  }
  vTaskDelete(NULL);
}



static void PID_control(void* pvParameters) {
  long number_task3 = 0;
  int PID_control_LED_state = 0;
  int PID_control_counter = 0;
  while (1) {
    Serial << "\n";
    PID_control_counter++;
    unsigned long PID_control_time_begin = micros();
    for (PID_control_counter ; PID_control_counter <= 4000 ; PID_control_counter++) {
      number_task3++;
      PID_control_LED_state = !PID_control_LED_state;
    digitalWrite(PID_control_pin, PID_control_LED_state);
    Serial << "PID_control\n";
    }
   unsigned long PID_control_time_end = micros();
   unsigned long PID_control_time_left_over = PID_control_time_end - PID_control_time_begin;
    if ( PID_control_time_left_over / 1000 >= 0 ) {
      vTaskDelay( (PID_control_time_left_over / 1000) / portTICK_PERIOD_MS);
    }
  }
  
  vTaskDelete(NULL);
}


static void Analog_Read(void* pvParameters) {
  long number_task4 = 0;
  int Analog_Read_LED_state = 0;
  int Analog_Read_counter = 0;
  while (1) {
  Serial << "\n";
  Analog_Read_counter++;
  unsigned long Analog_Read_time_begin = micros();

    for (Analog_Read_counter ; Analog_Read_counter <= 200 ; Analog_Read_counter++) {
      number_task4++;
      Analog_Read_LED_state = !Analog_Read_LED_state;
      digitalWrite(Analog_Read_LED_pin, Analog_Read_LED_state);
      Serial << "Analog_Read\n";
    }
    
   unsigned long Analog_Read_time_end = micros();
   unsigned long Analog_Read_time_left_over = Analog_Read_time_end - Analog_Read_time_begin;
    if ( Analog_Read_time_left_over / 1000 >= 0 ) {
      vTaskDelay( (Analog_Read_time_left_over / 1000) / portTICK_PERIOD_MS);
    }
  }
  vTaskDelete(NULL);
}

static void Idling_free_task(void* pvParameters) {
  long number_Idle  = 0;
  int TaskIDLE_LED_state = 0;
  while (1)
  {
   
    TaskIDLE_LED_state = !TaskIDLE_LED_state;
    digitalWrite(TaskIDLE_LED_pin, TaskIDLE_LED_state);
    Serial << "Idling_free_task\n";
   
    vTaskDelay(80 / portTICK_PERIOD_MS );
  }
  vTaskDelete(NULL);
}