/*
 * SPDX-FileCopyrightText: 2024-2025 Espressif Systems (Shanghai) CO LTD
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <ctype.h>
#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/uart.h"
#include "esp_log.h"
#include "direction_controller.h"

static const char *TAG = "Direction Controller";

#define DIRECTION_TASK_STACK_SIZE   (1024 * 4)

/* UART Configuration */
#define UART_PORT_NUM       UART_NUM_0
#define UART_BUF_SIZE       256
#define INPUT_BUF_SIZE      64


/**
 * @brief Print help message with available directions
 */
static void print_help(void)
{
    printf("\n========================================\n");
    printf("    方向控制系统 (Direction Control)\n");
    printf("========================================\n");
    printf("可用方向 (Available directions):\n");
    size_t direction_count = direction_controller_get_direction_count();
    for (size_t i = 0; i < direction_count; i++) {
        const direction_controller_direction_t *dir = direction_controller_get_direction(i);
        if (dir == NULL) {
            continue;
        }
        if (dir->angle == 0.0f) {
            printf("  %s/%s - %.1f° [默认位置]\n", dir->name_cn, dir->name_en, dir->angle);
        } else {
            printf("  %s/%s - %.1f°\n", dir->name_cn, dir->name_en, dir->angle);
        }
    }
    printf("----------------------------------------\n");
    printf("命令 (Commands):\n");
    printf("  help - 显示帮助信息\n");
    printf("  pos  - 显示当前位置\n");
    printf("  cal  - 校准正北位置\n");
    printf("========================================\n\n");
}

/**
 * @brief Print current position
 */
static void print_current_position(void)
{
    const direction_controller_direction_t *dir = direction_controller_get_current_direction();
    float angle = direction_controller_get_current_angle();
    if (dir != NULL) {
        printf("当前位置: %s (%.1f°)\n", dir->name_cn, angle);
    } else {
        printf("当前位置: 未知 (%.1f°)\n", angle);
    }
}

/**
 * @brief Direction control task - handles user input
 */
static void direction_control_task(void *arg)
{
    ESP_LOGI(TAG, "Direction control task started");
    
    print_help();
    
    char input_buf[INPUT_BUF_SIZE];
    int input_idx = 0;
    uint8_t data;
    
    printf("请输入方向 > ");
    fflush(stdout);
    
    while (1) {
        /* Read one byte at a time */
        int len = uart_read_bytes(UART_PORT_NUM, &data, 1, pdMS_TO_TICKS(100));
        
        if (len > 0) {
            /* Echo character */
            uart_write_bytes(UART_PORT_NUM, (const char*)&data, 1);
            
            /* Handle Enter key (CR or LF) */
            if (data == '\r' || data == '\n') {
                uart_write_bytes(UART_PORT_NUM, "\r\n", 2);
                
                if (input_idx > 0) {
                    input_buf[input_idx] = '\0';

                    /* Trim leading/trailing whitespace */
                    char *cmd = input_buf;
                    while (*cmd != '\0' && isspace((unsigned char)*cmd)) {
                        cmd++;
                    }
                    char *end = cmd + strlen(cmd);
                    while (end > cmd && isspace((unsigned char)*(end - 1))) {
                        end--;
                    }
                    *end = '\0';

                    if (*cmd != '\0') {
                        const direction_controller_direction_t *dir = NULL;
                        direction_controller_cmd_t cmd_type =
                            direction_controller_parse_command(cmd, &dir);

                        switch (cmd_type) {
                        case DIRECTION_CONTROLLER_CMD_HELP:
                            print_help();
                            break;
                        case DIRECTION_CONTROLLER_CMD_POSITION:
                            print_current_position();
                            break;
                        case DIRECTION_CONTROLLER_CMD_CALIBRATE: {
                            printf("开始校准...\n");
                            esp_err_t err = direction_controller_calibrate(NULL);
                            if (err == ESP_OK) {
                                printf("校准完成\n");
                                print_current_position();
                            } else if (err == ESP_ERR_TIMEOUT) {
                                printf("校准失败: 未检测到微动开关\n");
                            } else if (err == ESP_ERR_INVALID_STATE) {
                                printf("校准失败: 未配置校准开关\n");
                            } else {
                                printf("校准失败: 错误码 %d\n", err);
                            }
                            break;
                        }
                        case DIRECTION_CONTROLLER_CMD_DIRECTION:
                            if (dir != NULL) {
                                printf("目标方向: %s (%s, %.1f°)\n",
                                       dir->name_cn, dir->name_en, dir->angle);
                                if (direction_controller_rotate_to_angle(dir->angle) != ESP_OK) {
                                    printf("旋转失败\n");
                                }
                            }
                            break;
                        default:
                            printf("未知方向: '%s'\n", cmd);
                            printf("输入 'help' 查看可用方向\n");
                            break;
                        }
                    }
                }
                
                input_idx = 0;
                printf("\n请输入方向 > ");
                fflush(stdout);
            }
            /* Handle Backspace */
            else if (data == '\b' || data == 0x7F) {
                if (input_idx > 0) {
                    input_idx--;
                    uart_write_bytes(UART_PORT_NUM, " \b", 2);
                }
            }
            /* Regular character */
            else if (input_idx < INPUT_BUF_SIZE - 1) {
                input_buf[input_idx++] = data;
            }
        }
    }
}

/**
 * @brief Initialize UART for user input
 */
static void uart_init(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_DISABLE,
        .source_clk = UART_SCLK_DEFAULT,
    };
    
    ESP_ERROR_CHECK(uart_driver_install(UART_PORT_NUM, UART_BUF_SIZE * 2, 0, 0, NULL, 0));
    ESP_ERROR_CHECK(uart_param_config(UART_PORT_NUM, &uart_config));
}

void app_main(void)
{
    ESP_LOGI(TAG, "========================================");
    ESP_LOGI(TAG, "  Direction Control System");
    ESP_LOGI(TAG, "  Default position: South (南)");
    ESP_LOGI(TAG, "========================================");
    
    /* Initialize peripherals */
    direction_controller_calibration_t calibration = {
        .enabled = true,
        .gpio_num = GPIO_NUM_39,
        .active_level = 0,
        .pullup_en = false,
        .pulldown_en = false,
        .calibration_angle = 180.0f,
        .step_angle_deg = 5.0f,
        .max_sweep_deg = 360.0f,
        .search_cw = true,
        .settle_delay_ms = 10
    };
    direction_controller_config_t config = {
        .directions = NULL,
        .direction_count = 0,
        .initial_angle = 0.0f,
        .rotate_speed_us = 0,
        .init_stepper_gpio = true,
        .calibration = &calibration
    };
    direction_controller_init(&config);
    uart_init();
    
    ESP_LOGI(TAG, "Initial position: South (0°)");

    /* Start direction control task */
    xTaskCreate(direction_control_task, "direction_control_task", 
                DIRECTION_TASK_STACK_SIZE, NULL, 5, NULL);
}