/*
 * 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 "esp_log.h"
#include "driver/uart.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 "lvgl.h"

#include "dshot_esc_encoder.h"
#include "display.h"
#include "ui.h"
#include "motor.h"


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

static const uint16_t throttle = 100;

static const uint16_t OUTPUT_MIN = 0;
static const uint16_t OUTPUT_MAX = 150;

static const float KP = 0.12; // Proportional gain
static const float KI = 0.0003; // Integral gain
static const float KD = 0; // Derivative gain

extern "C" void app_main(void) {
    srand((unsigned int)esp_timer_get_time());

    esc_telemetry_t telemetry;
    uint16_t real_rpm = 0;

    AutoPID myPID(&real_rpm, &throttle, OUTPUT_MIN, OUTPUT_MAX, KP, KI, KD);

    init_display();

    build_ui();
    
    init_motor();
    update_throttle(throttle);
    
    while(1) {
        send_dshot_packet();
        
        uint8_t length = 0;
        ESP_ERROR_CHECK(uart_get_buffered_data_len(ESC_UART_NUM, (size_t*)&length));
        
        if(length >= 10) {
            if(parse_telemetry(&telemetry)) {
                real_rpm = telemetry.rpm / (uint16_t) CONFIG_MOTOR_POLECOUNT;

                myPID.run();
                //ESP_LOGI(TAG, "eRPM returned is: %d\n", telemetry.rpm);
                //ESP_LOGI(TAG, "real RPM returned is: %d\n", real_rpm);
                update_rpm_readout(real_rpm);
            }
        }
    }
}