// SharedGeometry.cpp
#include "SharedGeometry/SharedGeometry.h"
#include <stdexcept>
#include <cmath>
#include <algorithm>

// 如果没有定义M_PI，则定义它
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

// ==================== Vector2D 实现 ====================

Vector2D::Vector2D() : X(0.0), Y(0.0) {}

Vector2D::Vector2D(double x, double y) : X(x), Y(y) {}

Vector2D Vector2D::operator+(const Vector2D &other) const
{
    return Vector2D(X + other.X, Y + other.Y);
}

Vector2D Vector2D::operator-(const Vector2D &other) const
{
    return Vector2D(X - other.X, Y - other.Y);
}

Vector2D Vector2D::operator*(double scalar) const
{
    return Vector2D(X * scalar, Y * scalar);
}

double Vector2D::distanceTo(const Vector2D &other) const
{
    double dx = X - other.X;
    double dy = Y - other.Y;
    return std::sqrt(dx * dx + dy * dy);
}

double Vector2D::length() const
{
    return std::sqrt(X * X + Y * Y);
}

Vector2D Vector2D::normalized() const
{
    double len = length();
    if (len > 0.0)
    {
        return Vector2D(X / len, Y / len);
    }
    return Vector2D(0.0, 0.0);
}

Vector2D Vector2D::Zero()
{
    return Vector2D(0.0, 0.0);
}

double Vector2D::Distance(const Vector2D &v1, const Vector2D &v2)
{
    return std::sqrt(std::pow(v2.X - v1.X, 2) + std::pow(v2.Y - v1.Y, 2));
}

double Vector2D::Cross(const Vector2D &other) const
{
    return X * other.Y - Y * other.X;
}

double Vector2D::Dot(const Vector2D &other) const
{
    return X * other.X + Y * other.Y;
}

// ==================== Pose7 实现 ====================

Pose7::Pose7() : tx(0.0), ty(0.0), tz(0.0), qx(0.0), qy(0.0), qz(0.0), qw(1.0) {}

// ==================== PoseCalculator 实现 ====================

Pose7 PoseCalculator::CalculatePoseAndQuaternion(const Vector2D &start, const Vector2D &end)
{
    Pose7 pose;
    pose.tx = start.X;
    pose.ty = start.Y;
    pose.tz = 0.0;

    Vector2D direction = end - start;
    double length = direction.length();

    if (length > 0.0)
    {
        direction = direction * (1.0 / length);
        double angle = std::atan2(direction.Y, direction.X);

        // 转换为四元数（绕Z轴旋转）
        double halfAngle = angle * 0.5;
        pose.qw = std::cos(halfAngle);
        pose.qz = std::sin(halfAngle);
        pose.qx = 0.0;
        pose.qy = 0.0;
    }
    else
    {
        pose.qw = 1.0;
        pose.qx = 0.0;
        pose.qy = 0.0;
        pose.qz = 0.0;
    }

    return pose;
}

std::vector<double> PoseCalculator::CalculatePoseAndQuaternionArray(double x1, double y1, double x2, double y2)
{
    double x_D = x1;
    double y_D = y1;
    double x_G = x2;
    double y_G = y2;

    // 方向向量
    double dx = x_G - x_D;
    double dy = y_G - y_D;

    double v_norm = std::sqrt(dx * dx + dy * dy);

    // 使用小的epsilon值而不是直接比较0
    if (std::abs(v_norm) < 1e-10)
    {
        throw std::invalid_argument("两个点位置相同，无法计算姿态。");
    }

    // 单位化方向向量
    double vx = dx / v_norm;
    double vy = dy / v_norm;

    // 四元数（绕Z轴旋转）
    double theta = std::atan2(vy, vx);
    double halfTheta = theta / 2.0;
    double qw = std::cos(halfTheta);
    double qx = 0.0;
    double qy = 0.0;
    double qz = std::sin(halfTheta);

    // 返回长度为7的数组
    return {x_D, y_D, 0.0, qw, qx, qy, qz};
}

double PoseCalculator::MMToM(double MM)
{
    return Round(MM / 1000.0, 6);
}

double PoseCalculator::Round(double value, int decimals)
{
    double factor = std::pow(10.0, decimals);
    return std::round(value * factor) / factor;
}

std::string PoseCalculator::ToString(double value, int decimals)
{
    std::ostringstream oss;
    oss << std::fixed << std::setprecision(decimals) << value;
    return oss.str();
}

C_ObjStates PoseCalculator::CalculatePoseAndQuaternion_C_ObjStates(const std::string &i,
                                                                   double x1, double y1,
                                                                   double x2, double y2)
{
    // 如果需要实现这个方法，需要C_ObjStates的定义
    // 这里先抛出一个异常，提醒需要实现
    // throw std::runtime_error("C_ObjStates需要额外的定义，请提供C_ObjStates类的定义");

    // 示例代码（需要C_ObjStates类）：

    double x_D = MMToM(x1);
    double y_D = MMToM(y1);
    double x_G = MMToM(x2);
    double y_G = MMToM(y2);

    double dx = x_G - x_D;
    double dy = y_G - y_D;
    double v_norm = std::sqrt(dx * dx + dy * dy);

    if (std::abs(v_norm) < 1e-10)
    {
        throw std::invalid_argument("两个点位置相同，无法计算姿态。");
    }

    double vx = dx / v_norm;
    double vy = dy / v_norm;
    double theta = std::atan2(vy, vx);
    double halfTheta = theta / 2.0;
    double qw = std::cos(halfTheta);
    double qx = 0.0;
    double qy = 0.0;
    double qz = std::sin(halfTheta);

    C_ObjStates c_ObjStates;
    c_ObjStates.i = i;
    c_ObjStates.tx = ToString(x_D);
    c_ObjStates.ty = ToString(y_D);
    c_ObjStates.tz = "0";
    c_ObjStates.qx = ToString(qx);
    c_ObjStates.qy = ToString(qy);
    c_ObjStates.qz = ToString(qz);
    c_ObjStates.qw = ToString(qw);

    return c_ObjStates;
}