package com.example.m20_gamepad.service

import com.example.m20_gamepad.network.ProtocolClient
import com.example.m20_gamepad.network.models.ControlCommands
import com.example.m20_gamepad.network.models.StatusReport
import kotlinx.coroutines.flow.MutableStateFlow
import kotlinx.coroutines.flow.StateFlow
import kotlinx.coroutines.flow.asStateFlow

/**
 * 连接状态机 + 运动状态机。负责连接状态流转校验与运动指令前置条件校验。
 *
 * - 连接状态：校验 [ProtocolClient.State] 转换合法性，避免非法跳转。
 * - 运动状态：依据 proto.md 2.3 节，正向流程 `空闲 -> 站立 -> RL控制`，
 *   仅 RL 控制状态允许步态切换与轴指令。
 *
 * 使用方式：ProtocolClient 状态变化时调用 [updateConnectionState]，
 * 收到状态上报时调用 [updateFromReport]；下发运动指令前调用
 * [requestMotionTransition] / [canSwitchGait] / [canSendAxisCommand] 校验。
 */
class RobotConnection {

    // ==================== 连接状态机 ====================

    private val _connectionState = MutableStateFlow(ProtocolClient.State.DISCONNECTED)
    val connectionState: StateFlow<ProtocolClient.State> = _connectionState.asStateFlow()

    val isConnected: Boolean get() = _connectionState.value == ProtocolClient.State.CONNECTED

    /**
     * 更新连接状态。非法转换被忽略并返回 false。
     * 合法转换：避免从 ERROR/TIMEOUT 直接跳到 CONNECTED，必须先 DISCONNECTED。
     */
    fun updateConnectionState(target: ProtocolClient.State): Boolean {
        val current = _connectionState.value
        if (!isValidConnectionTransition(current, target)) return false
        _connectionState.value = target
        return true
    }

    /** 连接状态流转合法性。 */
    private fun isValidConnectionTransition(
        from: ProtocolClient.State,
        to: ProtocolClient.State
    ): Boolean {
        if (from == to) return true
        return when (from) {
            ProtocolClient.State.DISCONNECTED ->
                to == ProtocolClient.State.CONNECTING
            ProtocolClient.State.CONNECTING ->
                to == ProtocolClient.State.CONNECTED ||
                to == ProtocolClient.State.ERROR ||
                to == ProtocolClient.State.DISCONNECTED
            ProtocolClient.State.CONNECTED ->
                to == ProtocolClient.State.TIMEOUT ||
                to == ProtocolClient.State.ERROR ||
                to == ProtocolClient.State.DISCONNECTED
            ProtocolClient.State.TIMEOUT ->
                to == ProtocolClient.State.CONNECTED ||
                to == ProtocolClient.State.DISCONNECTED ||
                to == ProtocolClient.State.ERROR
            ProtocolClient.State.ERROR ->
                to == ProtocolClient.State.DISCONNECTED
        }
    }

    // ==================== 运动状态机 ====================

    private val _motionState = MutableStateFlow(ControlCommands.MOTION_IDLE)
    val motionState: StateFlow<Int> = _motionState.asStateFlow()

    /** 是否允许下发步态切换（仅 RL 控制）。 */
    val canSwitchGait: Boolean
        get() = _motionState.value == ControlCommands.MOTION_RL_CONTROL

    /** 是否允许下发轴指令（仅 RL 控制）。 */
    val canSendAxisCommand: Boolean
        get() = _motionState.value == ControlCommands.MOTION_RL_CONTROL

    /** 是否允许下发运动状态转换指令（需已连接）。 */
    val canSendMotionControl: Boolean
        get() = _connectionState.value == ProtocolClient.State.CONNECTED

    /**
     * 校验运动状态转换请求。
     *
     * 依据 proto.md 2.3：`空闲 -> 站立 -> RL控制` 正向流程；其余常见回退（站立->空闲/
     * RL控制->站立/各状态->趴下->站立）允许。软急停/开机阻尼可从任意活跃态进入，恢复到站立。
     */
    fun requestMotionTransition(target: Int): MotionTransitionResult {
        if (!isConnected) return MotionTransitionResult.REJECTED_NOT_CONNECTED
        val current = _motionState.value
        if (current == target) return MotionTransitionResult.REJECTED_ALREADY_IN_STATE
        if (!isValidMotionTransition(current, target)) {
            return MotionTransitionResult.REJECTED_INVALID_TRANSITION
        }
        // 本地乐观更新：实际状态以下一次状态上报为准
        _motionState.value = target
        return MotionTransitionResult.ACCEPTED
    }

    /** 运动状态转换合法性。 */
    private fun isValidMotionTransition(from: Int, to: Int): Boolean {
        // 阻尼态（软急停/开机阻尼）可从任意活跃态进入
        if (to == ControlCommands.MOTION_DAMPING || to == ControlCommands.MOTION_BOOT_DAMPING) {
            return true
        }
        // 趴下可从站立/RL控制/空闲进入
        if (to == ControlCommands.MOTION_LIE_DOWN) {
            return from == ControlCommands.MOTION_IDLE ||
                from == ControlCommands.MOTION_STAND ||
                from == ControlCommands.MOTION_RL_CONTROL
        }
        return when (from) {
            ControlCommands.MOTION_IDLE ->
                to == ControlCommands.MOTION_STAND
            ControlCommands.MOTION_STAND ->
                to == ControlCommands.MOTION_IDLE ||
                to == ControlCommands.MOTION_RL_CONTROL
            ControlCommands.MOTION_RL_CONTROL ->
                to == ControlCommands.MOTION_STAND
            ControlCommands.MOTION_DAMPING,
            ControlCommands.MOTION_BOOT_DAMPING ->
                to == ControlCommands.MOTION_STAND
            ControlCommands.MOTION_LIE_DOWN ->
                to == ControlCommands.MOTION_STAND
            else -> false
        }
    }

    /**
     * 从状态上报同步真实运动状态。服务端上报值优先于本地乐观更新。
     */
    fun updateFromReport(report: StatusReport) {
        report.basicStatus?.let { _motionState.value = it.motionState }
    }

    /** 断开/重置：清回空闲与未连接。 */
    fun reset() {
        _connectionState.value = ProtocolClient.State.DISCONNECTED
        _motionState.value = ControlCommands.MOTION_IDLE
    }
}

/** 运动状态转换校验结果。 */
enum class MotionTransitionResult {
    /** 允许，已本地更新状态。 */
    ACCEPTED,
    /** 未连接。 */
    REJECTED_NOT_CONNECTED,
    /** 状态机不允许该转换。 */
    REJECTED_INVALID_TRANSITION,
    /** 已处于目标状态。 */
    REJECTED_ALREADY_IN_STATE
}
