#include <FreeRTOS.h> // https://iot-kmutnb.github.io/blogs/stm32/stm32f4_arduino_freertos/
#define TX D12
#define RX D13
#define IO1 D14
#define IO2 D15
#define IO3 D11
#define DATA_BIT 8
#define BAUD_RATE 9600 // 1e6 //

TaskHandle_t idFrameDetectTask_handle = NULL;
SemaphoreHandle_t sem_frame_id = NULL;

typedef struct
{
  // uint8_t head = 0;
  byte data = B00000000;
  uint8_t flag = 0;
  // uint8_t tail = 1;
} Frame_t;

typedef struct
{
  uint8_t tx_pin = 0;
  uint8_t rx_pin  = 0;
  uint8_t data_bit = 8;
  uint16_t sampling_us = 0;
} UART_CFG_t;

Frame_t frame_raw; 
UART_CFG_t uart_cfg;

Frame_t frame_test[14];
current_frame_id = 0;

void uartBegin(uint8_t _tx_pin, uint8_t _rx_pin) //(uint16_t _baud_rate, uint8_t _tx_pin, uint8_t _rx_pin, UART_CFG_t* _uart_cfg)
{
  pinMode(_tx_pin,OUTPUT);
  pinMode(_rx_pin,INPUT);
  // _uart_cfg->sampling_us = (uint8_t)(1e6*(1/(double)_baud_rate));
}

void transmitter(int _tx_pin, byte _data, uint8_t _flag, int _data_size, uint16_t _sampling_us)
{
  bool _bits[_data_size];
  // 1. Write start bit of frame
  digitalWrite(_tx_pin, LOW);
  delayMicroseconds(_sampling_us);
  // 2. Write 8 data bits
  for (uint8_t _i=_data_size-1; _i>=0; _i--)
  {
    _bits[_i] = (_data >> _i) & 1;
    Serial.print("bits[");
    Serial.print(_i);
    Serial.print("] is ");
    Serial.print(_bits[_i]);
    Serial.println();
    digitalWrite(_tx_pin, _bits[_i]);
    delayMicroseconds(_sampling_us);
    if (_i == 0) break;
  }
  // 3. Write data type flag (0 for data, 1 for new address)
  digitalWrite(_tx_pin, _flag);
  delayMicroseconds(_sampling_us);
  // 4. Write stop bit of frame.
  digitalWrite(_tx_pin, HIGH);
}

// ------------ Task Create here -----------
void idFrameDetectTask( void *pvParameters )
{
  uint8_t io_num = 3;
  pinMode(IO1, INPUT); pinMode(IO2, INPUT); pinMode(IO3, INPUT);
  while(1)
  {
    if (digitalRead(IO1) == HIGH)
    {
      current_frame_id = 0;
      xSemaphoreGive(sem_frame_id);
    }
    if (digitalRead(IO2) == HIGH)
    {
      current_frame_id = 1;
      xSemaphoreGive(sem_frame_id);
    }
    if (digitalRead(IO3) == HIGH)
    {
      current_frame_id = 2;
      xSemaphoreGive(sem_frame_id);
    }
    vTaskDelay(100);
  }
  vTaskDelete(NULL);
};

void setup() 
{
  // put your setup code here, to run once:
  Serial.begin(9600);
  uart_cfg.tx_pin = D12; uart_cfg.rx_pin = D13; 
  uart_cfg.sampling_us = (uint16_t)(1e6*(1/(double)BAUD_RATE));
  // Serial.print("sampling_us = ");
  // Serial.print(uart_cfg.sampling_us,DEC);
  // Serial.println(" us.");
  // Define data frame
  frame_test[0].data=0x01; frame_test[0].flag = 0;
  frame_test[1].data=0x02; frame_test[1].flag = 0;
  frame_test[2].data=0x03; frame_test[2].flag = 1;
  frame_test[3].data=0x04; frame_test[3].flag = 0;
  frame_test[4].data=0x05; frame_test[4].flag = 0;
  frame_test[5].data=0x06; frame_test[5].flag = 0;
  frame_test[6].data=0x07; frame_test[6].flag = 0;
  frame_test[7].data=0x01; frame_test[7].flag = 1;
  frame_test[8].data=0x02; frame_test[8].flag = 1;
  frame_test[9].data=0x08; frame_test[9].flag = 0;
  frame_test[10].data=0x09; frame_test[10].flag = 0;
  frame_test[11].data=0x0a; frame_test[11].flag = 1;
  frame_test[12].data=0x0b; frame_test[12].flag = 0;
  frame_test[13].data=0x00; frame_test[13].flag = 1;
  // transmitter(TX, data, flag, size, sampling_us)
  //transmitter(uart_cfg.tx_pin, frame_test[7].data, frame_test[7].flag, uart_cfg.data_bit, uart_cfg.sampling_us); 
  sem_frame_id = xSemaphoreCreateBinary();
  xTaskCreate(idFrameDetectTask, "idFrameDetectTask", configMINIMAL_STACK_SIZE+100, NULL, 1, &idFrameDetectTask_handle);
}

void loop() 
{
  // transmitter(uart_cfg.tx_pin, frame_raw.data, 1, 8, uart_cfg.sampling_us); 
  // Serial.println("----------------");
  if(xSemaphoreTake(sem_frame_id, 0))
  {
    transmitter(uart_cfg.tx_pin, frame_test[current_frame_id].data, frame_test[current_frame_id].flag, uart_cfg.data_bit, uart_cfg.sampling_us);
  }
  vTaskDelay(100);
}