#include <math.h>
#include "quad_pos_control_math.h"
float vector2f_norm(const Vector2f *v) {
    return sqrtf(v->x * v->x + v->y * v->y);
}

Vector2f vector2f_add(const Vector2f *v1, const Vector2f *v2) {
    Vector2f res = {v1->x + v2->x, v1->y + v2->y};
    return res;
}

Vector2f vector2f_sub(const Vector2f *v1, const Vector2f *v2) {
    Vector2f res = {v1->x - v2->x, v1->y - v2->y};
    return res;
}

float vector2f_dot(const Vector2f *v1, const Vector2f *v2) {
    return v1->x * v2->x + v1->y * v2->y;
}

Vector2f vector2f_normalized(const Vector2f *v) {
    float norm = vector2f_norm(v);
    if (norm < 1e-6f) return (Vector2f){0.0f, 0.0f};
    Vector2f res = {v->x / norm, v->y / norm};
    return res;
}

Vector2f vector2f_scale(const Vector2f *v, float s) {
    Vector2f res = {v->x * s, v->y * s};
    return res;
}

// 限幅函数
Vector2f constrainXY(const Vector2f *v0, const Vector2f *v1, float max) {
    Vector2f sum = vector2f_add(v0, v1);
    float sum_norm = vector2f_norm(&sum);

    if (sum_norm <= max) {
        return sum;
    } else if (vector2f_norm(v0) >= max) {
        return vector2f_scale(v0, max / vector2f_norm(v0));
    } else if (vector2f_norm(v0) < 0.001f) {
        return vector2f_scale(v1, max / vector2f_norm(v1));
    } else {
        // 计算 v1 方向的单位向量
        Vector2f u1 = vector2f_normalized(v1);
        float m = vector2f_dot(&u1, v0);
        float c = vector2f_dot(v0, v0) - max * max;
        float discriminant = m * m - c;
        
        // 处理负判别式（数值稳定性）
        if (discriminant < 0) discriminant = 0;
        float s = -m + sqrtf(discriminant);
        return vector2f_add(v0, vector2f_scale(&u1, s));
    }
}