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

#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include "driver/uart.h"
#include "esp_log.h"
#include "stepper_motor.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

/* Current angle tracking (relative to South = 0°) */
static float s_current_angle = 0.0f;

/* Direction definitions (angles relative to South = 0°) */
typedef struct {
    const char *name_cn;        /* Chinese name */
    const char *name_en;        /* English abbreviation */
    float angle;                /* Angle relative to South */
} direction_t;

/* 
 * Direction angle mapping (South = 0°, clockwise positive)
 * 
 *              北 (180°)
 *               |
 *    西北(135°) |  东北(-135°/225°)
 *               |
 * 西(90°) ------+------ 东(-90°/270°)
 *               |
 *    西南(45°)  |  东南(-45°/315°)
 *               |
 *              南 (0°) [HOME]
 */
static const direction_t directions[] = {
    {"南",   "S",   0.0f},      /* South - Home position */
    {"西南", "SW",  45.0f},     /* Southwest */
    {"西",   "W",   90.0f},     /* West */
    {"西北", "NW",  135.0f},    /* Northwest */
    {"北",   "N",   180.0f},    /* North */
    {"东北", "NE",  -135.0f},   /* Northeast */
    {"东",   "E",   -90.0f},    /* East */
    {"东南", "SE",  -45.0f},    /* Southeast */
};
#define NUM_DIRECTIONS (sizeof(directions) / sizeof(directions[0]))

/**
 * @brief Calculate shortest rotation angle to target
 * 
 * @param current Current angle
 * @param target Target angle
 * @return Shortest rotation angle (positive = clockwise, negative = counter-clockwise)
 */
static float calculate_shortest_rotation(float current, float target)
{
    float diff = target - current;
    
    /* Normalize to -180 ~ 180 range */
    while (diff > 180.0f) {
        diff -= 360.0f;
    }
    while (diff < -180.0f) {
        diff += 360.0f;
    }
    
    return diff;
}

/**
 * @brief Rotate to specified direction
 * 
 * @param target_angle Target angle (relative to South)
 */
static void rotate_to_angle(float target_angle)
{
    float rotation = calculate_shortest_rotation(s_current_angle, target_angle);
    
    if (rotation == 0.0f) {
        ESP_LOGI(TAG, "Already at target position");
        return;
    }
    
    ESP_LOGI(TAG, "Rotating from %.1f° to %.1f° (rotation: %.1f°)", 
             s_current_angle, target_angle, rotation);
    
    stepper_rotate_angle_with_accel(rotation, STEPPER_SPEED_FAST);
    s_current_angle = target_angle;
    stepper_motor_power_off();
    
    ESP_LOGI(TAG, "Rotation complete, current position: %.1f°", s_current_angle);
}

/**
 * @brief Find direction by name
 * 
 * @param input User input string
 * @return Pointer to direction_t if found, NULL otherwise
 */
static const direction_t* find_direction(const char *input)
{
    for (int i = 0; i < NUM_DIRECTIONS; i++) {
        if (strcmp(input, directions[i].name_cn) == 0 ||
            strcasecmp(input, directions[i].name_en) == 0) {
            return &directions[i];
        }
    }
    return NULL;
}

/**
 * @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");
    printf("  南/S   - South    (0°)   [默认位置]\n");
    printf("  西南/SW - Southwest (45°)\n");
    printf("  西/W   - West     (90°)\n");
    printf("  西北/NW - Northwest (135°)\n");
    printf("  北/N   - North    (180°)\n");
    printf("  东北/NE - Northeast (-135°)\n");
    printf("  东/E   - East     (-90°)\n");
    printf("  东南/SE - Southeast (-45°)\n");
    printf("----------------------------------------\n");
    printf("命令 (Commands):\n");
    printf("  help - 显示帮助信息\n");
    printf("  pos  - 显示当前位置\n");
    printf("========================================\n\n");
}

/**
 * @brief Print current position
 */
static void print_current_position(void)
{
    const char *dir_name = "未知";
    for (int i = 0; i < NUM_DIRECTIONS; i++) {
        if (directions[i].angle == s_current_angle) {
            dir_name = directions[i].name_cn;
            break;
        }
    }
    printf("当前位置: %s (%.1f°)\n", dir_name, s_current_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 == ' ') cmd++;
                    char *end = cmd + strlen(cmd) - 1;
                    while (end > cmd && *end == ' ') *end-- = '\0';
                    
                    /* Process command */
                    if (strcmp(cmd, "help") == 0 || strcmp(cmd, "?") == 0) {
                        print_help();
                    } else if (strcmp(cmd, "pos") == 0) {
                        print_current_position();
                    } else {
                        const direction_t *dir = find_direction(cmd);
                        if (dir != NULL) {
                            printf("目标方向: %s (%s, %.1f°)\n", 
                                   dir->name_cn, dir->name_en, dir->angle);
                            rotate_to_angle(dir->angle);
                        } else {
                            printf("未知方向: '%s'\n", cmd);
                            printf("输入 'help' 查看可用方向\n");
                        }
                    }
                }
                
                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 */
    stepper_motor_gpio_init();
    uart_init();
    
    /* Set initial position to South (0°) */
    s_current_angle = 0.0f;
    stepper_motor_power_off();
    
    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);
}