// SharedGeometry.h
#ifndef SHARED_GEOMETRY_H
#define SHARED_GEOMETRY_H

#include <string>
#include <vector>
#include <cmath>
#include <sstream>
#include <iomanip>
#include "QuadrupedRobotSimulation/OPERATION.h"
// 2D向量类
class Vector2D
{
public:
    double X;
    double Y;

    Vector2D();
    Vector2D(double x, double y);

    // 运算符重载
    Vector2D operator+(const Vector2D &other) const;
    Vector2D operator-(const Vector2D &other) const;
    Vector2D operator*(double scalar) const;

    // 几何运算
    double distanceTo(const Vector2D &other) const;
    double length() const;
    Vector2D normalized() const;

    // 静态方法
    static Vector2D Zero();
    static double Distance(const Vector2D &v1, const Vector2D &v2);
    double Length() const { return length(); }
    Vector2D Normalize() const { return normalized(); }

    // 叉积和点积
    double Cross(const Vector2D &other) const;
    double Dot(const Vector2D &other) const;
};

// 姿态类（简化版）
class Pose7
{
public:
    double tx;
    double ty;
    double tz;
    double qx;
    double qy;
    double qz;
    double qw;

    Pose7();
};

// 姿态计算器 - 兼顾两个程序的需求
class PoseCalculator
{
public:
    /**
     * 计算两个2D点之间的位置和姿态（四元数）
     * 适用于CrankSliderMechanism
     */
    static Pose7 CalculatePoseAndQuaternion(const Vector2D &start, const Vector2D &end);

    /**
     * 计算两个2D点之间的位置和姿态（四元数）
     * 返回double[7]数组，顺序为: [x, y, z, qw, qx, qy, qz]
     * 适用于BaseClass程序
     */
    static std::vector<double> CalculatePoseAndQuaternionArray(double x1, double y1, double x2, double y2);

    /**
     * 毫米转米
     */
    static double MMToM(double MM);

    /**
     * 四舍五入辅助函数
     */
    static double Round(double value, int decimals = 6);

    /**
     * 数字转字符串
     */
    static std::string ToString(double value, int decimals = 6);

    /**
     * 计算姿态并返回C_ObjStates对象（如果需要）
     */
    static C_ObjStates CalculatePoseAndQuaternion_C_ObjStates(const std::string &i,
                                                              double x1, double y1,
                                                              double x2, double y2);
};

#endif // SHARED_GEOMETRY_H