package com.example.m20_gamepad.service

import com.example.m20_gamepad.network.models.ControlCommands
import kotlin.math.cos
import kotlin.math.sin

/**
 * 摇杆输入 -> 协议轴指令映射。
 *
 * 摇杆库 angle = atan2(-thumbOffsetY, -thumbOffsetX)，弧度范围 [-PI, PI]：
 * - 0    = 左
 * - PI/2 = 上
 * - PI   = 右
 * - -PI/2 = 下
 *
 * 协议轴定义：
 * - X 正 = 前进，Y 正 = 左移，Yaw 正 = 逆时针（左转）
 *
 * 映射公式：
 * - 左摇杆 X(前后) = sin(angle) * power/100
 * - 左摇杆 Y(左右) = cos(angle) * power/100
 * - 右摇杆 Yaw     = cos(angle) * power/100
 */
object JoystickController {

    /** 左摇杆角度/强度 -> 轴指令 JSON（X=前后, Y=左右） */
    fun leftStickToAxisCommand(angle: Double, power: Double): String {
        val scale = power / 100.0
        val x = sin(angle) * scale
        val y = cos(angle) * scale
        return ControlCommands.axisCommand(x = x, y = y)
    }

    /** 右摇杆角度/强度 -> 轴指令 JSON（Yaw=偏航） */
    fun rightStickToAxisCommand(angle: Double, power: Double): String {
        val yaw = cos(angle) * power / 100.0
        return ControlCommands.axisCommand(yaw = yaw)
    }

    /** 双摇杆合并 -> 轴指令 JSON（左摇杆控制 X/Y，右摇杆控制 Yaw） */
    fun dualStickToAxisCommand(
        leftAngle: Double, leftPower: Double,
        rightAngle: Double, rightPower: Double
    ): String {
        val leftScale = leftPower / 100.0
        val rightScale = rightPower / 100.0
        val x = sin(leftAngle) * leftScale
        val y = cos(leftAngle) * leftScale
        val yaw = cos(rightAngle) * rightScale
        return ControlCommands.axisCommand(x = x, y = y, yaw = yaw)
    }

    /** 零速度指令（松手时发送，保持连接活跃） */
    fun zeroAxisCommand(): String = ControlCommands.axisCommand()
}