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

#include "domain_internal.h"

/* 角度统一归一化到 (-180, 180]，为目录匹配和最短旋转提供唯一表示。 */
float domain_angle_normalize(float angle)
{
    while (angle > 180.0f) {
        angle -= 360.0f;
    }
    while (angle <= -180.0f) {
        angle += 360.0f;
    }
    return angle;
}

float domain_angle_shortest_rotation(float current, float target)
{
    /* 结果限制到 [-180, 180]，正负号决定方向，避免不必要的整圈运动。 */
    float diff = target - current;

    while (diff > 180.0f) {
        diff -= 360.0f;
    }
    while (diff < -180.0f) {
        diff += 360.0f;
    }

    return diff;
}