#include "robot_fsm.hpp"
#include <conio.h>  // 用于键盘输入 (Windows)
#include <random>   // 用于模拟一些输入

// 机器人控制器构造函数
RobotFsm::RobotFsm() : currentState(RobotState::STOPPED) {
    initTransitionTable();
}

// 初始化状态转换表
void RobotFsm::initTransitionTable() {
    // 从任意状态都可以转换到停止状态
    transitionTable[{RobotState::STOPPED, ControlCommand::STOP}] = RobotState::STOPPED;
    transitionTable[{RobotState::MOVING_FORWARD, ControlCommand::STOP}] = RobotState::STOPPED;
    transitionTable[{RobotState::MOVING_BACKWARD, ControlCommand::STOP}] = RobotState::STOPPED;
    transitionTable[{RobotState::TURNING_LEFT, ControlCommand::STOP}] = RobotState::STOPPED;
    transitionTable[{RobotState::TURNING_RIGHT, ControlCommand::STOP}] = RobotState::STOPPED;
    transitionTable[{RobotState::MOVING_UP, ControlCommand::STOP}] = RobotState::STOPPED;
    transitionTable[{RobotState::MOVING_DOWN, ControlCommand::STOP}] = RobotState::STOPPED;
    
    // 从停止状态转换到其他状态
    transitionTable[{RobotState::STOPPED, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD;
    transitionTable[{RobotState::STOPPED, ControlCommand::BACKWARD}] = RobotState::MOVING_BACKWARD;
    transitionTable[{RobotState::STOPPED, ControlCommand::LEFT}] = RobotState::TURNING_LEFT;
    transitionTable[{RobotState::STOPPED, ControlCommand::RIGHT}] = RobotState::TURNING_RIGHT;
    transitionTable[{RobotState::STOPPED, ControlCommand::UP}] = RobotState::MOVING_UP;
    transitionTable[{RobotState::STOPPED, ControlCommand::DOWN}] = RobotState::MOVING_DOWN;
    
    // 从运动状态转换到其他运动状态 TODO: 不支持直接切换
    // transitionTable[{RobotState::MOVING_FORWARD, ControlCommand::BACKWARD}] = RobotState::MOVING_BACKWARD;
    // transitionTable[{RobotState::MOVING_FORWARD, ControlCommand::LEFT}] = RobotState::TURNING_LEFT;
    // transitionTable[{RobotState::MOVING_FORWARD, ControlCommand::RIGHT}] = RobotState::TURNING_RIGHT;
    // transitionTable[{RobotState::MOVING_BACKWARD, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD;
    // transitionTable[{RobotState::MOVING_BACKWARD, ControlCommand::LEFT}] = RobotState::TURNING_LEFT;
    // transitionTable[{RobotState::MOVING_BACKWARD, ControlCommand::RIGHT}] = RobotState::TURNING_RIGHT;
    // 其他可能的状态转换...
    // transitionTable[{RobotState::TURNING_LEFT, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD;
    // transitionTable[{RobotState::TURNING_RIGHT, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD;
}

// 添加输入设备
void RobotFsm::addInputDevice(InputType type, std::unique_ptr<InputDevice> device) {
    inputDevices[type] = std::move(device);
    std::cout << "Added input device: " << inputDevices[type]->getDeviceName() << std::endl;
}

// 状态机主循环
void RobotFsm::run() {
    std::cout << "Robot controller started. Press 'q' to quit." << std::endl;
    std::cout << "Keyboard controls: WASD to move, Q to quit, U to up, D to down" << std::endl;
    
    while (true) {
        // 检查所有输入设备
        ControlCommand cmd = ControlCommand::NONE;
        for (const auto& [type, device] : inputDevices) {
            ControlCommand deviceCmd = device->getCommand();
            if (deviceCmd != ControlCommand::NONE) {
                cmd = deviceCmd;
                break;  // 优先级：先获取到的命令先执行
            }
        }
        
        // 处理退出命令
        if (cmd == ControlCommand::NONE) {
            // 检查键盘的退出命令（特殊处理）
            if (_kbhit()) {
                char key = _getch();
                if (key == 'q' || key == 'Q') {
                    std::cout << "Exiting robot controller..." << std::endl;
                    processCommand(ControlCommand::STOP);
                    break;
                }
            }
        } else {
            processCommand(cmd);
        }
        
        // 执行当前状态的动作
        executeStateAction();
        
        // 控制循环频率
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
}

// 处理命令并转换状态
void RobotController::processCommand(ControlCommand cmd) {
    if (cmd == ControlCommand::NONE) return;
    
    auto it = transitionTable.find({currentState, cmd});
    if (it != transitionTable.end()) {
        RobotState newState = it->second;
        if (newState != currentState) {
            std::cout << "State changed: " << stateToString(currentState) 
                      << " -> " << stateToString(newState) << std::endl;
            currentState = newState;
        }
    }
}

// 执行当前状态对应的动作
void RobotController::executeStateAction() {
    // 在实际应用中，这里会发送相应的控制信号给机器人
    switch (currentState) {
        case RobotState::STOPPED:
            // 发送停止命令
            break;
        case RobotState::MOVING_FORWARD:
            // 发送前进命令
            break;
        case RobotState::MOVING_BACKWARD:
            // 发送后退命令
            break;
        case RobotState::TURNING_LEFT:
            // 发送左转命令
            break;
        case RobotState::TURNING_RIGHT:
            // 发送右转命令
            break;
        case RobotState::MOVING_UP:
            // 发送上升命令
            break;
        case RobotState::MOVING_DOWN:
            // 发送下降命令
            break;
    }
}

// 状态转换为字符串
std::string RobotController::stateToString(RobotState state) const {
    switch (state) {
        case RobotState::STOPPED: return "STOPPED";
        case RobotState::MOVING_FORWARD: return "MOVING_FORWARD";
        case RobotState::MOVING_BACKWARD: return "MOVING_BACKWARD";
        case RobotState::TURNING_LEFT: return "TURNING_LEFT";
        case RobotState::TURNING_RIGHT: return "TURNING_RIGHT";
        case RobotState::MOVING_UP: return "MOVING_UP";
        case RobotState::MOVING_DOWN: return "MOVING_DOWN";
        default: return "UNKNOWN";
    }
}

// 键盘输入处理
ControlCommand KeyboardInput::getCommand() {
    if (_kbhit()) {
        char key = _getch();
        switch (key) {
            case 'w': case 'W': return ControlCommand::FORWARD;
            case 's': case 'S': return ControlCommand::BACKWARD;
            case 'a': case 'A': return ControlCommand::LEFT;
            case 'd': case 'D': return ControlCommand::RIGHT;
            case 'u': case 'U': return ControlCommand::UP;
            case 'j': case 'J': return ControlCommand::DOWN;  // 使用J代替D避免与后退冲突
            case ' ': return ControlCommand::STOP;  // 空格键停止
            default: return ControlCommand::NONE;
        }
    }
    return ControlCommand::NONE;
}

// 手柄输入处理（模拟实现）
ControlCommand GamepadInput::getCommand() {
    // 实际应用中这里会读取游戏手柄的输入
    // 这里使用随机值模拟手柄输入，仅作示例
    static std::random_device rd;
    static std::mt19937 gen(rd());
    static std::uniform_int_distribution<> dist(0, 100);
    
    int val = dist(gen);
    
    if (val < 10) return ControlCommand::FORWARD;
    else if (val < 20) return ControlCommand::BACKWARD;
    else if (val < 30) return ControlCommand::LEFT;
    else if (val < 40) return ControlCommand::RIGHT;
    else if (val < 45) return ControlCommand::UP;
    else if (val < 50) return ControlCommand::DOWN;
    else if (val < 55) return ControlCommand::STOP;
    else return ControlCommand::NONE;
}

// 语音输入处理（模拟实现）
ControlCommand VoiceInput::getCommand() {
    // 实际应用中这里会处理语音识别结果
    // 这里使用随机值模拟语音输入，仅作示例
    static std::random_device rd;
    static std::mt19937 gen(rd());
    static std::uniform_int_distribution<> dist(0, 100);
    
    int val = dist(gen);
    
    if (val < 5) return ControlCommand::FORWARD;
    else if (val < 10) return ControlCommand::BACKWARD;
    else if (val < 15) return ControlCommand::LEFT;
    else if (val < 20) return ControlCommand::RIGHT;
    else if (val < 23) return ControlCommand::UP;
    else if (val < 26) return ControlCommand::DOWN;
    else if (val < 29) return ControlCommand::STOP;
    else return ControlCommand::NONE;
}
    