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

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "driver/rmt_tx.h"
#include "driver/gpio.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 DSHOT_ESC_GPIO_NUM           22
#define DSHOT_ESC_TELEMETRY_GPIO_NUM 27

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

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 = 1000 / 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);
    //vTaskDelay(pdMS_TO_TICKS(1000));
}

/**
 * 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 = DSHOT_ESC_GPIO_NUM,
        .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();
}

/**
 * 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;
    }
}

void app_main(void) {
    init_rmt_esc_tx();
    
    throttle.throttle = 200;
    xTaskCreate(&v_telemetry_packet_func, "v_telemetry_packet_func", 2048, NULL, 5, NULL);
    
    gpio_dump_io_configuration(stdout, (1ULL << 27));

    printf("==============================\n\n");
    
    // Testing gpio setup
    gpio_config_t io_conf = {};
    
    io_conf.intr_type = GPIO_INTR_DISABLE;
    io_conf.mode = GPIO_MODE_INPUT_OUTPUT;
    io_conf.pin_bit_mask = (1ULL<<DSHOT_ESC_TELEMETRY_GPIO_NUM);
    io_conf.pull_down_en = GPIO_PULLDOWN_ENABLE;
    io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
    
    gpio_config(&io_conf);
    
    gpio_dump_io_configuration(stdout, (1ULL << 27));

    while(1) {
        send_dshot_packet();
    }
}