#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include "queue.h"

#include <stdint.h>
#include "tinystdio.h"

#include "usart.h"

#ifdef RS485_USART

#undef DBG
#define DBG if(0)

#define RS485_BUFSIZE   256

static uint8_t buf[RS485_BUFSIZE];


static void rs485_thread(void *arg)
{
    (void)arg;

    while (1) {
        /* Get packet */
        uint16_t n = 0;
        xQueueReceive(rs485RxQ, &buf[n++], portMAX_DELAY);
        while (xQueueReceive(rs485RxQ, &buf[n], 10) == pdTRUE && n < RS485_BUFSIZE) {
            n++;
        }
        /* Echo inverted packet */
        uint16_t i = 0;
        while (i < n) {
            uint8_t byte = ~buf[i++];
            if (xQueueSend(rs485TxQ, &byte, 1000) == pdTRUE) {
                rs485_enable_tx();
            } else {
                DBG printf("[echo] rx data lost\r\n");
                return;
            }
        }
    }
}

void rs485echo_init(void)
{
    xTaskCreate(rs485_thread, ( char * ) "rs485_thr", configMINIMAL_STACK_SIZE,
        NULL, tskIDLE_PRIORITY, NULL);
}
#endif