#include <freertos/FreeRTOS.h>
#include <freertos/queue.h>
#include <freertos/event_groups.h>
#include <driver/can.h>
#include <nvs_flash.h>
#include <esp_task_wdt.h>
#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include "can.h"
#include "hci.h"

const int can_tx_pin = GPIO_NUM_0;
const int can_rx_pin = GPIO_NUM_2;

struct
{
    can_timing_config_t timing;
    can_filter_config_t filter;
    can_message_t periodic_messages[8];
    uint8_t flags; /* initialized, rx */
} can_config;

can_general_config_t config =
{
    .mode = CAN_MODE_NORMAL,
    .tx_io = can_tx_pin,
    .rx_io = can_rx_pin,
    .clkout_io = -1,
    .bus_off_io = -1,
    .tx_queue_len = 10,
    .rx_queue_len = 10,
    .alerts_enabled = CAN_ALERT_NONE,
    .clkout_divider = 0
};

const uint8_t CAN_FLAG_INIT = 1 << 0;
const uint8_t CAN_FLAG_RX_ON = 1 << 1;
static QueueHandle_t can_rx_queue;
static EventBits_t can_reinstall_done = 1 << 0;
static EventGroupHandle_t can_event_group;
struct can_rx_event
{
    uint8_t event;
};

enum
{
    EVENT_CAN_RX_OFF = 0,
    EVENT_CAN_RX_ON = 1,
    EVENT_CAN_REINSTALL = 2
};


int can_init()
{
    esp_err_t err;
    /* Setup default timing */
    can_config.timing.brp = 8;
    can_config.timing.tseg_1 = 5;
    can_config.timing.tseg_2 = 4;
    can_config.timing.sjw = 1;
    /* Setup default filter */
    can_config.filter.acceptance_code = 0;
    can_config.filter.acceptance_mask = 0xffffffff;
    can_config.filter.single_filter = 1;
    err = can_driver_install(&config, &can_config.timing, &can_config.filter);

    if(err != ESP_OK)
    {
        printf("ERR CAN init failed!\n");
        return -1;
    }

    err = can_start();
    if(err != ESP_OK)
    {
        printf("ERR CAN init failed!\n");
        return -1;
    }

    can_event_group = xEventGroupCreate();
    can_config.flags |= CAN_FLAG_INIT;
    return 0;
}

int can_reinstall()
{
    esp_err_t err;
    can_stop();
    can_driver_uninstall();
    /* Setup default timing */
    err = can_driver_install(&config, &can_config.timing, &can_config.filter);
    if(err != ESP_OK)
        return -1;
    err = can_start();
    if(err != ESP_OK)
        return -1;
    return 0;
}

int parse_message_format(can_message_t *msg, const char *fmt)
{
    uint32_t id;
    char remote;
    memset(msg, 0, sizeof(*msg));
    sscanf(fmt, "%x#%c", &id, &remote);
    msg->flags |= CAN_MSG_FLAG_SS;
    if(remote == 'R')
    {
        msg->flags |= CAN_MSG_FLAG_RTR;
        msg->identifier = id;
        msg->data_length_code = 0;
    }
    else
    {
        int data[8];
        int ret = sscanf(fmt, "%x#%2x%2x%2x%2x%2x%2x%2x%2x",
            &id, &data[0], &data[1], &data[2], &data[3],
            &data[4], &data[5], &data[6], &data[7]);
        if(ret < 1)
            return -1;
        int len = ret - 1;
        msg->identifier = id;
        msg->data_length_code = len;
        for(int i = 0; i < len; i++)
            msg->data[i] = data[i];
    }

    return 0;
}

void can_command()
{
    char *cmd = strtok(NULL, " ");
    esp_err_t err;
    /* Make sure we have a command */
    if(!cmd)
        goto einval;
    /*
     * Compare commande against known commands and handle them, otherwise tell
     * the user it was invalid.
     */
    if(strcmp(cmd, "help") == 0)
    {
        printf("OK\n");
        printf(
            "Available commands:\n"
            "Source clock: 80 MHz\n"
            "CAN bitrate = 80 000 000 / brp / (1 + tseg_1 + tseg_2)\n"
            "\n"
            "can help - write this text\n"
            "can rx on/off - enable or disable RX\n"
            "can send <id>#<data in hex> - e.g. send 13f#02e8\n"
            "can config brp [value] - get or set current can brp (2-128, even)\n"
            "can config tseg_1 [value] - get or set current can tseg_1 (1-16)\n"
            "can config tseg_2 [value] - get or set current can tseg_2 (1-8)\n"
            "can config sjw [value] - get or set current can sjw (1-4)\n"
            "\n");
    }
    else if(strcmp(cmd, "rx") == 0)
    {
        const char *arg = strtok(NULL, " ");

        if(strcmp(arg, "on") == 0)
            can_rx_on();
        else if(strcmp(arg, "off") == 0)
            can_rx_off();
        else
            goto einval;
    }
    else if(strcmp(cmd, "send") == 0)
    {
        can_message_t msg;
        const char *msg_str = strtok(NULL, " ");

        memset(&msg, 0, sizeof(msg));
        if(parse_message_format(&msg, msg_str) < 0)
            goto einval;
        struct timeval current_time;
        gettimeofday(&current_time, NULL);
        /* printf("seconds : %ld\nmicro seconds : %ld", current_time.tv_sec, current_time.tv_usec); */
        /* unsigned long currentMillis = xTaskGetTickCount(); */
        unsigned long time_in_micros = 1000000 * current_time.tv_sec + current_time.tv_usec;
        printf("T: %ld SENT %x#", time_in_micros, msg.identifier);
        for(int i = 0; i < msg.data_length_code; i++)
            printf("%02x", msg.data[i]);
        printf("\n");
        err = can_transmit(&msg, 0);
        if(err != ESP_OK)
        {
            printf("ERR Transmit failed!\n");
            /*
             * If we get to many transmission errors due to non-acked frames
             * then the bus will go into an error state, restart it by
             * reinstalling it.
             */
            can_send_reinstall();
            return;
        }

    }
        else if(strcmp(cmd, "sendex") == 0)
    {
        can_message_t msg;
        const char *msg_str = strtok(NULL, " ");

        memset(&msg, 0, sizeof(msg));
        if(parse_message_format(&msg, msg_str) < 0)
            goto einval;
        msg.extd = 1;
        struct timeval current_time;
        gettimeofday(&current_time, NULL);
        /* printf("seconds : %ld\nmicro seconds : %ld", current_time.tv_sec, current_time.tv_usec); */
        /* unsigned long currentMillis = xTaskGetTickCount(); */
        unsigned long time_in_micros = 1000000 * current_time.tv_sec + current_time.tv_usec;
        printf("T: %ld SENT %x#", time_in_micros, msg.identifier);
        for(int i = 0; i < msg.data_length_code; i++)
            printf("%02x", msg.data[i]);
        printf("\n");
        err = can_transmit(&msg, 0);
        if(err != ESP_OK)
        {
            printf("ERR Transmit failed!\n");

            /*
             * If we get to many transmission errors due to non-acked frames
             * then the bus will go into an error state, restart it by
             * reinstalling it.
             */
            can_send_reinstall();

            return;
        }

    }
    else if(strcmp(cmd, "config") == 0)
    {
        /* Read value argument */
        const char *arg = strtok(NULL, " ");
        if(!arg)
            goto einval;
        if(strcmp(arg, "brp") == 0)
        {
            const char *value_str = strtok(NULL, " ");
            if(!value_str)
                printf("OK %d\n", can_config.timing.brp);

            else
            {
                int value = atoi(value_str);

                if(value < 2 || value > 127 || value % 2)
                    goto einval;
                can_config.timing.brp = value;
                can_send_reinstall();
            }
        }
        else if(strcmp(arg, "tseg_1") == 0)
        {
            const char *value_str = strtok(NULL, " ");
            if(!value_str)
                printf("OK %d\n", can_config.timing.tseg_1);
            else
            {
                int value = atoi(value_str);
                if(value < 1 || value > 16 )
                    goto einval;
                can_config.timing.tseg_1 = value;
                can_send_reinstall();
            }
        }
        else if(strcmp(arg, "tseg_2") == 0)
        {
            const char *value_str = strtok(NULL, " ");
            if(!value_str)
                printf("OK %d\n", can_config.timing.tseg_2);
            else
            {
                int value = atoi(value_str);
                if(value < 1 || value > 8 )
                    goto einval;
                can_config.timing.tseg_2 = value;
                can_send_reinstall();
            }
        }
        else if(strcmp(arg, "sjw") == 0)
        {
            const char *value_str = strtok(NULL, " ");
            if(!value_str)
                printf("OK %d\n", can_config.timing.sjw);
            else
            {
                int value = atoi(value_str);
                if(value < 1 || value > 4 )
                    goto einval;
                can_config.timing.sjw = value;
                can_send_reinstall();
            }
        }
        else
            goto einval;
    }
    else if(strcmp(cmd, "status") == 0)
    {
    }
    else
        goto einval;
    return;

einval:
    printf("INVALID COMMAND");
    return;
}

void can_rx_off()
{
    struct can_rx_event event =
    {
        .event = EVENT_CAN_RX_OFF
    };
    xQueueSendToBack(can_rx_queue, &event, 0);
}

void can_rx_on()
{
    struct can_rx_event event =
    {
        .event = EVENT_CAN_RX_ON
    };
    xQueueSendToBack(can_rx_queue, &event, 0);
}

void can_send_reinstall()
{
    struct can_rx_event event =
    {
        .event = EVENT_CAN_REINSTALL
    };
    xQueueSendToBack(can_rx_queue, &event, 0);
    /* Wait for reinstallation done */
    xEventGroupWaitBits(can_event_group, can_reinstall_done, 1, 1, 1000 * portTICK_PERIOD_MS);
}

void can_rx_thread(void *parameters)
{
    /* Check that CAN initialized correctly */
    while(!(can_config.flags & CAN_FLAG_INIT))
        vTaskDelay(100 * portTICK_PERIOD_MS);
    esp_task_wdt_delete(xTaskGetCurrentTaskHandle());
    can_rx_queue = xQueueCreate(10, sizeof(struct can_rx_event));
    int rx_running = 0;

    while(1)
    {
        can_message_t msg;
        while(can_receive(&msg, 10) == ESP_OK)
        {
            /* AUTOPING */
            /* can_transmit(&msg, 0); */
            /* AUTOPING */
            if(rx_running)
            {

                struct timeval tvr;
                gettimeofday(&tvr, NULL);
                /* unsigned long currentMillis = xTaskGetTickCount(); */
                unsigned long time_in_micros = 1000000 * tvr.tv_sec + tvr.tv_usec;
                printf("T: %ld CAN RX: %x#", time_in_micros, msg.identifier);
                if(msg.flags & CAN_MSG_FLAG_RTR)
                    printf("R");
                else
                    for(int i = 0; i < msg.data_length_code; i++)
                        printf("%02x", msg.data[i]);
                /*
                if (msg.extd)
                    printf("Message is in Extended Format\n");
                else
                    printf("Message is in Standard Format\n");
                */

                printf("\n");
            }
        }

        struct can_rx_event event;
        if(xQueueReceive(can_rx_queue, &event, 0))
        {
            if(event.event == EVENT_CAN_RX_OFF)
            {
                rx_running = 0;
                printf("OK\n");
            }

            else if(event.event == EVENT_CAN_RX_ON)
            {
                rx_running = 1;
                printf("OK\n");
            }

            else if(event.event == EVENT_CAN_REINSTALL)
            {
                if(can_reinstall() < 0)
                    printf("ERR Unknown error\n");
                else
                    printf("OK\n");
                /* Also perform a full recovery which helps on bus-off recovery */
                can_initiate_recovery();

                while(1)
                {
                    twai_status_info_t info;
                    twai_get_status_info(&info);
                    if(info.state != CAN_STATE_RECOVERING)
                        break;
                }

                xEventGroupSetBits(can_event_group, can_reinstall_done);
            }
        }
    }
}