/*
 * SPDX-FileCopyrightText: 2021-2022 Espressif Systems (Shanghai) CO LTD
 *
 * SPDX-License-Identifier: Unlicense OR CC0-1.0
 */

#include <inttypes.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"

#include "unity.h"
#include "unity_test_runner.h"

#include "esp_log.h"
#include "driver/rmt_tx.h"
#include "driver/uart.h"
#include "driver/gpio.h"

#include "hal/spi_types.h"
#include "esp_lcd_panel_ops.h"
#include "driver/spi_common.h"
#include "esp_lcd_panel_io.h"
#include "esp_lcd_panel_commands.h"
#include "esp_lcd_ili9341.h"

#include "dshot_esc_encoder.h"

#if CONFIG_IDF_TARGET_ESP32H2
#define DSHOT_ESC_RESOLUTION_HZ 32000000 // 32MHz resolution, DSHot protocol needs a relative high resolution
#else
#define DSHOT_ESC_RESOLUTION_HZ 40000000 // 40MHz resolution, DSHot protocol needs a relative high resolution
#endif

#define GPIO_ESC_CTRL CONFIG_ESC_CTRL_PIN
#define GPIO_ESC_RX   CONFIG_TELEMETRY_RX_PIN
#define UART_NUM      UART_NUM_2
#define LCD_SPI_HOST  SPI2_HOST

#define GPIO_TFT_MISO CONFIG_TFT_MISO_PIN
#define GPIO_TFT_MOSI CONFIG_TFT_MOSI_PIN
#define GPIO_TFT_SCKL CONFIG_TFT_SCKL_PIN
#define GPIO_TFT_CS   CONFIG_TFT_CS_PIN
#define GPIO_TFT_DC   CONFIG_TFT_DC_PIN
#define GPIO_TFT_BL   CONFIG_TFT_BL_PIN // Backlight
#define TFT_HRES      CONFIG_TFT_HRES
#define TFT_VRES      CONFIG_TFT_VRES
#define TFT_BPP       CONFIG_TFT_BPP

#define ESP_INTR_FLAG_DEFAULT        0

static const char *TAG = "spincoat-plater-firmware";

static SemaphoreHandle_t refresh_finish = NULL;

static QueueHandle_t uart_queue = NULL;
const int uart_buffer_size = (1024 * 2);

rmt_encoder_handle_t dshot_encoder = NULL;
rmt_channel_handle_t esc_chan = NULL;

rmt_transmit_config_t tx_config = {
    .loop_count = 0,
};

dshot_esc_throttle_t throttle = {
    .throttle = 0,
    .telemetry_req = false, // telemetry is not supported in this example
};

/**
 * Sends a telemetry packet at a set, constant interval
 */
void v_telemetry_packet_func(void *pvParameters) {
    TickType_t frequency = 10 / portTICK_PERIOD_MS;
    TickType_t last_wake_time = xTaskGetTickCount();
    while(1) {
        throttle.telemetry_req = true;
        vTaskDelayUntil(&last_wake_time, frequency);
    }
}

/**
 * Sends zero throttle to arm ESC for control. Stop/delete this task once the ESC has armed.
 */
void v_initialize_esc_throttle_func(void *pvParameters) {
    while(1) {
        ESP_ERROR_CHECK(rmt_transmit(esc_chan, dshot_encoder, &throttle, sizeof(throttle), &tx_config));
    }
}

/**
 * Starts task *v_initialize_esc_throttle_func()* for a few seconds and then destroys it.
 * This function takes care of the arming stage of ESC control.
 */
void initialize_esc_throttle(void) {
    TaskHandle_t v_init_throttle_handle = NULL;
    xTaskCreate(&v_initialize_esc_throttle_func, "v_init_throttle_func", 2048, NULL, 5, &v_init_throttle_handle);
    vTaskDelay(pdMS_TO_TICKS(5000));
    vTaskDelete(v_init_throttle_handle);
}

/**
 * Initialize the RMT system in preparation for sending DSHOT packets to the connected ESC.
 */
void init_rmt_esc_tx(void) {
    ESP_LOGI(TAG, "Create RMT TX channel");
    rmt_tx_channel_config_t tx_chan_config = {
        .clk_src = RMT_CLK_SRC_DEFAULT, // select a clock that can provide needed resolution
        .gpio_num = GPIO_ESC_CTRL,
        .mem_block_symbols = 64,
        .resolution_hz = DSHOT_ESC_RESOLUTION_HZ,
        .trans_queue_depth = 10, // set the number of transactions that can be pending in the background
    };
    ESP_ERROR_CHECK(rmt_new_tx_channel(&tx_chan_config, &esc_chan));

    ESP_LOGI(TAG, "Install Dshot ESC encoder");
    dshot_esc_encoder_config_t encoder_config = {
        .resolution = DSHOT_ESC_RESOLUTION_HZ,
        .baud_rate = 300000, // DSHOT300 protocol
        .post_delay_us = 50, // extra delay between each frame
    };
    ESP_ERROR_CHECK(rmt_new_dshot_esc_encoder(&encoder_config, &dshot_encoder));

    ESP_LOGI(TAG, "Enable RMT TX channel");
    ESP_ERROR_CHECK(rmt_enable(esc_chan));

    ESP_LOGI(TAG, "Start ESC by sending zero throttle for a while...");
    initialize_esc_throttle();
}

/**
 * Initialize the UART receive pin so that we can receive telemetry data from the connected ESC.
 */
void init_telemetry_uart_rx(void) {
    uart_config_t uart_config = {
        .baud_rate = 115200,
        .data_bits = UART_DATA_8_BITS,
        .parity = UART_PARITY_DISABLE,
        .stop_bits = UART_STOP_BITS_1,
        .flow_ctrl = UART_HW_FLOWCTRL_CTS_RTS,
        .rx_flow_ctrl_thresh = 122,
    };
    
    ESP_ERROR_CHECK(uart_driver_install(UART_NUM, uart_buffer_size, uart_buffer_size, 10, &uart_queue, 0));
    ESP_ERROR_CHECK(uart_param_config(UART_NUM, &uart_config));
    ESP_ERROR_CHECK(uart_set_pin(UART_NUM, UART_PIN_NO_CHANGE, GPIO_ESC_RX, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
}

/**
 * Callback for the TFT LCD, notifying when the screen is ready for another chunk of data and
 * releasing the drawing semaphore.
 */
IRAM_ATTR static bool notify_refresh_ready(esp_lcd_panel_io_handle_t panel_io, esp_lcd_panel_io_event_data_t *edata, void *user_ctx)
{
    BaseType_t need_yield = pdFALSE;

    xSemaphoreGiveFromISR(refresh_finish, &need_yield);
    return (need_yield == pdTRUE);
}

/**
 * Draws a test bitmap of stripes of colors to the LCD.
 */
static void test_draw_bitmap(esp_lcd_panel_handle_t panel_handle)
{
    refresh_finish = xSemaphoreCreateBinary();
    TEST_ASSERT_NOT_NULL(refresh_finish);

    uint16_t row_line = TFT_VRES / TFT_BPP;
    uint8_t byte_per_pixel = TFT_BPP / 8;
    uint8_t *color = (uint8_t *)heap_caps_calloc(1, row_line * TFT_VRES * byte_per_pixel, MALLOC_CAP_DMA);
    TEST_ASSERT_NOT_NULL(color);

    for (int j = 0; j < TFT_BPP; j++) {
        for (int i = 0; i < row_line * TFT_HRES ; i++) {
            for (int k = 0; k < byte_per_pixel; k++) {
                color[i * byte_per_pixel + k] = (SPI_SWAP_DATA_TX(BIT(j), TFT_BPP) >> (k * 8)) & 0xff;
            }
        }
        TEST_ESP_OK(esp_lcd_panel_draw_bitmap(panel_handle, 0, j * row_line, TFT_HRES , (j + 1) * row_line, color));
        xSemaphoreTake(refresh_finish, portMAX_DELAY);
    }
    free(color);
    vSemaphoreDelete(refresh_finish);
}

/**
 * Initializes the SPI LCD in preparation for writing graphics to it.
 */
void init_spi_lcd(void) {
    ESP_LOGI(TAG, "Turn on backlight");
    
    gpio_config_t io_conf = {
        .pin_bit_mask = (1ULL << GPIO_TFT_BL),
        .mode = GPIO_MODE_OUTPUT,
        .pull_up_en = GPIO_PULLUP_ENABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .intr_type = GPIO_INTR_DISABLE
    };
    
    gpio_config(&io_conf);
    gpio_set_level(GPIO_TFT_BL, 1);

    ESP_LOGI(TAG, "Initialize SPI bus");
    const spi_bus_config_t bus_config = ILI9341_PANEL_BUS_SPI_CONFIG(GPIO_TFT_SCKL,
                                        GPIO_TFT_MOSI, TFT_HRES * 80 * TFT_BPP / 8);
    TEST_ESP_OK(spi_bus_initialize(LCD_SPI_HOST, &bus_config, SPI_DMA_CH_AUTO));

    ESP_LOGI(TAG, "Install panel IO");
    esp_lcd_panel_io_handle_t io_handle = NULL;
    const esp_lcd_panel_io_spi_config_t io_config = ILI9341_PANEL_IO_SPI_CONFIG(GPIO_TFT_CS, GPIO_TFT_DC,

    TEST_ESP_OK(esp_lcd_new_panel_io_spi((esp_lcd_spi_bus_handle_t)LCD_SPI_HOST, &io_config, &io_handle));

    ESP_LOGI(TAG, "Install ili9341 panel driver");
    esp_lcd_panel_handle_t panel_handle = NULL;
    const esp_lcd_panel_dev_config_t panel_config = {
        .reset_gpio_num = -1, // Shared with Touch reset
#if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0)
        .color_space = ESP_LCD_COLOR_SPACE_BGR,
#elif ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(6, 0, 0)
        .rgb_endian = LCD_RGB_ENDIAN_BGR,
#else
        .rgb_ele_order = LCD_RGB_ELEMENT_ORDER_BGR,
#endif
        .bits_per_pixel = TFT_BPP,
    };
    TEST_ESP_OK(esp_lcd_new_panel_ili9341(io_handle, &panel_config, &panel_handle));
    TEST_ESP_OK(esp_lcd_panel_reset(panel_handle));
    TEST_ESP_OK(esp_lcd_panel_init(panel_handle));
    TEST_ESP_OK(esp_lcd_panel_mirror(panel_handle, true, true));
#if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0)
    TEST_ESP_OK(esp_lcd_panel_disp_off(panel_handle, false));
#else
    TEST_ESP_OK(esp_lcd_panel_disp_on_off(panel_handle, true));
#endif
    
    ESP_LOGI(TAG, "Finished init of spi LCD.");
    ESP_LOGI(TAG, "Drawing bitmap.");;
    test_draw_bitmap(panel_handle);
    vTaskDelay(pdMS_TO_TICKS(3000));

    // Tear it back down, move this into a function to clean up after ourselves if it's ever needed.
    ESP_LOGI(TAG, "Destroying and cleaning up LCD/SPI handles.");
    gpio_reset_pin(GPIO_TFT_BL);
    TEST_ESP_OK(esp_lcd_panel_del(panel_handle));
    TEST_ESP_OK(esp_lcd_panel_io_del(io_handle));
    TEST_ESP_OK(spi_bus_free(LCD_SPI_HOST));
}

/**
 * Sends a DSHOT packet via the RMT. Make sure the RMT channel has been initialized
 * by calling *init_rmt_esc_tx()*
 */
void send_dshot_packet(void) {
    ESP_ERROR_CHECK(rmt_transmit(esc_chan, dshot_encoder, &throttle, sizeof(throttle), &tx_config));
    
    if(throttle.telemetry_req == true) {
        throttle.telemetry_req = false;
    }
}

/**
 * Calculate one step of the crc8 and return it
 */
uint8_t update_crc8(uint8_t crc, uint8_t crc_seed){
    uint8_t crc_u, i;
    crc_u = crc;
    crc_u ^= crc_seed;
    for ( i=0; i<8; i++) crc_u = ( crc_u & 0x80 ) ? 0x7 ^ ( crc_u << 1 ) : ( crc_u << 1 );
    return (crc_u);
}

/**
 * Calculate the entire crc8 for a KISS frame and return it for validation against the
 * transmitted crc8
 */
uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen){
    uint8_t crc = 0, i;
    for( i=0; i<BufLen; i++) crc = update_crc8(Buf[i], crc);
    return (crc);
}

/**
 * Parse the KISS telemetry frame and check the crc8
 * TODO: Do more with the data than print it
 */
void parse_telemetry(void) {
    uint8_t frame_size = 10;
    uint8_t data[128];
    // get data
    uint8_t length = uart_read_bytes(UART_NUM, data, frame_size, 100);
    
    uart_flush(UART_NUM);

    if(length < 10) return;
    
    // chop out just the payload
    uint8_t payload[128];
    uint8_t payload_length = (frame_size - 1);
    for(uint8_t i = 0; i < payload_length; i++) {
        payload[i] = data[i];
    }
    
    // calculate the crc8
    uint8_t expected_crc8 = get_crc8(payload, payload_length);
    uint8_t received_crc8 = (uint8_t) data[frame_size - 1];
    
    if(expected_crc8 != received_crc8) return;
    
    for(uint8_t i = 0; i < length; i++) {
        printf("%d - %d\n", i, data[i]);
    }
    printf("--------------------\n");
    printf("expected: %d\n", expected_crc8);
    printf("received: %d\n", received_crc8);
    printf("======================\n");
}

void app_main(void) {
    init_spi_lcd();
    
    init_rmt_esc_tx();
    throttle.throttle = 300;
    
    xTaskCreate(&v_telemetry_packet_func, "v_telemetry_packet_func", 2048, NULL, 1, NULL);

    init_telemetry_uart_rx();

    while(1) {
        send_dshot_packet();
        
        uint8_t length = 0;
        ESP_ERROR_CHECK(uart_get_buffered_data_len(UART_NUM, (size_t*)&length));
        
        if(length >= 10) {
            parse_telemetry();
        }
    }
}