/*
 * 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 "AutoPID-for-ESP-IDF.h"

extern "C" {
    #include "dshot_esc_encoder.h"
    #include "display.h"
    #include "ui.h"
    #include "motor.h"
}

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

static double throttle = 0;

static double zero_offset = 100; // Value to offset the throttle by, to skip the
                                 // command values
static double OUTPUT_MIN = 0;
static double OUTPUT_MAX = 1948;

static double RPM_MAX = 250;
static double BANG_BANG_THRESHOLD = RPM_MAX + 200;

static double KP = 0.015;
static double KI = 0.8;
static double KD = 0.0;

static bool motor_running = false;

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

    esc_telemetry_t telemetry;
    double target = 100;
    double real_rpm = 0;

    AutoPID myPID(&real_rpm, &target, &throttle, OUTPUT_MIN, OUTPUT_MAX, KP, KI, KD);
    myPID.setTimeStep(100);
    myPID.setBangBang(400); // if you mess with this, you may melt your circuit

    init_display();

    vTaskDelay(pdMS_TO_TICKS(100));
    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();
                update_throttle(throttle + zero_offset);

                update_rpm_readout(real_rpm);
                ESP_LOGI(TAG, "eRPM: %d, RPM: %.2f, SetPoint: %.2f, Output: %.2f",
                    telemetry.rpm,
                    real_rpm,
                    target,
                    throttle + zero_offset); // Log the values

                if(myPID.atSetPoint(10)) {
                    ESP_LOGI(TAG, "At setpoint.");
                }

                vTaskDelay(pdMS_TO_TICKS(100));

                if(real_rpm >= 30) {
                  motor_running = true;
                } else {
                  motor_running = false;
                }
            }
        }
    }
}