#include <stdio.h>
#include <stdlib.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include "esp_err.h"
#include "esp_log.h"
#include "driver/twai.h"  
#include "modbus.h"

#define TX_GPIO_NUM                     42
#define RX_GPIO_NUM                     41
#define RX_BUF_LEN                      (8)

static const char *TAG = "twal";

static const twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(TX_GPIO_NUM, RX_GPIO_NUM, TWAI_MODE_NORMAL);
static const twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS();
static const twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();


static void twai_receive_task(void *arg)
{
    ESP_LOGI(TAG, "twai_receive_task");

    uint8_t rx_data[RX_BUF_LEN];
    int rx_len = 0;
    twai_message_t message;

    while (1)
    {
        /* 接受一个modbus帧 */
        if (twai_receive(&message, portMAX_DELAY) == ESP_OK) {
            /* 远程帧据不处理 */
            if (message.rtr) {
                ESP_LOGI(TAG, "remote frame, return");
            }
            else if (message.data_length_code != 4) {
                ESP_LOGI(TAG, "Invalid data length");
            }
            else {
                modbus_data->angle = (message.data[1] << 8) | message.data[0];
                modbus_data->speed = (message.data[3] << 8) | message.data[2];
                ESP_LOGI(TAG, "angle: %d, speed: %d", modbus_data->angle, modbus_data->speed);
            }
        }
        else 
        {
            ESP_LOGI(TAG, "Failed to receive message");
        }

        /* 因为设计的缺陷，这里要手动调用modbus的回调来调方向 */
        ModBusWordWriteHook(MODBUS_ANGLE_ADDR, 1);
    }
}


static void twai_send_task(void *arg)
{
    ESP_LOGI(TAG, "twai_send_task");

    twai_message_t message;
    while (1)
    {
        message.identifier = 0xAAAA;
        message.extd = 1;
        message.data_length_code = 8;
        message.data[0] = modbus_data->encoder_accumul & 0xff;
        message.data[1] = (modbus_data->encoder_accumul >> 8) & 0xff;
        message.data[2] = (modbus_data->encoder_accumul >> 16) & 0xff;
        message.data[3] = (modbus_data->encoder_accumul >> 24) & 0xff;
        message.data[4] = modbus_data->encoder_accumur & 0xff;
        message.data[5] = (modbus_data->encoder_accumur >> 8) & 0xff;
        message.data[6] = (modbus_data->encoder_accumur >> 16) & 0xff;
        message.data[7] = (modbus_data->encoder_accumur >> 24) & 0xff;

        if (twai_transmit(&message, pdMS_TO_TICKS(1000)) != ESP_OK) {
            ESP_LOGI(TAG, "Failed to queue message for transmission");
        }

        vTaskDelay(pdMS_TO_TICKS(100));
    }
}


void twai_init(void)
{
    ESP_LOGI(TAG, "twai init");

    ESP_ERROR_CHECK(twai_driver_install(&g_config, &t_config, &f_config));
    ESP_ERROR_CHECK(twai_start());

    xTaskCreatePinnedToCore(twai_receive_task, "TWAI_rx", 4096, NULL, 9, NULL, tskNO_AFFINITY);
    xTaskCreatePinnedToCore(twai_send_task, "TWAI_tx", 4096, NULL, 8, NULL, tskNO_AFFINITY);
}