#include "FITKVec3DAlg.h"
#include <cmath>

namespace Core
{

    FITKPoint FITKCoreAPI Add(const FITKPoint& a, const FITKPoint& b, const double aFac, const double bFac)
    {
        double c[3] = { 0,0,0 };
 
        c[1] = a.y()*aFac + b.y()*bFac;
        c[0] = a.x()*aFac + b.x()*bFac;
        c[2] = a.z()*aFac + b.z()*bFac;

        // 创建并返回新的FITKPoint对象，存储结果
        return FITKPoint(c);
    }

    FITKPoint FITKCoreAPI Subtract(const FITKPoint& a, const FITKPoint& b, const double aFac, const double bFac )
    {
        double c[3] = { 0,0,0 };
 
        c[0] = a.x()*aFac - b.x()*bFac;
        c[1] = a.y()*aFac - b.y()*bFac;
        c[2] = a.z()*aFac - b.z()*bFac;

        // 创建并返回新的FITKPoint对象，存储结果
        return FITKPoint(c);
    }

    /**
     * 计算两个三维点（FITKPoint类型）的向量叉积，并返回新的FITKPoint对象。
     *
     * @param a 第一个输入向量（FITKPoint类型）
     * @param b 第二个输入向量（FITKPoint类型）
     * @return 叉积结果（FITKPoint类型）
     */
    FITKPoint CrossProduct(const FITKPoint& a, const FITKPoint& b)
    {
        double c[3] = { 0,0,0 };

        // 根据叉积定义计算各个分量
        c[0] = a.y() * b.z() - a.z() * b.y();
        c[1] = a.z() * b.x() - a.x() * b.z();
        c[2] = a.x() * b.y() - a.y() * b.x();

        // 创建并返回新的FITKPoint对象，存储叉积结果
        return FITKPoint(c);
    }

    /**
     * 计算两个三维点（FITKPoint类型）的向量点积，并返回标量结果。
     *
     * @param a 第一个输入向量（FITKPoint类型）
     * @param b 第二个输入向量（FITKPoint类型）
     * @return 点积结果（double类型）
     */
    double DotProduct(const FITKPoint & a, const FITKPoint & b)
    {
        // 根据点积定义计算标量结果
        return a.x() * b.x() + a.y() * b.y() + a.z() * b.z();
    }

    /**
     * 将三维点P绕给定轴线Axis按指定角度Angle进行旋转，并返回旋转后的FITKPoint对象。
     *
     * @param P 待旋转的三维点（FITKPoint类型）
     * @param Axis 旋转轴线
     * @param Angle 旋转角度，以弧度为单位（double类型）
     * @return 旋转后的三维点（FITKPoint类型）
     */
    FITKPoint RotateAroundAxis(const FITKPoint& P,  FITKVec3D& Axis, double Angle)
    {
        Axis.normalize();
        double cosTheta = std::cos(Angle);
        double sinTheta = std::sin(Angle);
        double kP = DotProduct(Axis, P); // 计算点P在旋转轴上的投影长度
        FITKPoint kCrossP = CrossProduct(Axis, P); // 计算点P相对于旋转轴的垂直向量

        // 应用罗德里格斯公式计算旋转后的点坐标
        FITKPoint pt(
            P.x() * cosTheta + kCrossP.x() * sinTheta + Axis.x() * kP * (1.0 - cosTheta),
            P.y() * cosTheta + kCrossP.y() * sinTheta + Axis.y() * kP * (1.0 - cosTheta),
            P.z() * cosTheta + kCrossP.z() * sinTheta + Axis.z() * kP * (1.0 - cosTheta)
        );

        return pt; // 返回旋转后的FITKPoint对象
    }

    double FITKCoreAPI Distance(const FITKPoint& a, const FITKPoint& b)
    {
        double dx = a.x() - b.x();
        double dy = a.y() - b.y();
        double dz = a.z() - b.z();
        return sqrt(dx*dx + dy * dy + dz * dz);
    }

    double FITKCoreAPI AngleBetweenVec(const FITKVec3D& v1, const FITKVec3D& v2)
    {
        //点乘
        double dp = DotProduct(v1, v2);
        //角度余弦值
        double arcAng = dp / (v1.getNorm()*v2.getNorm());
        return std::acos(arcAng);
    }

}