#include "rtcp_kinematics.h"

#include <cmath>

namespace {

constexpr double kPi = 3.141592653589793238462643383279502884;

double radians(double degrees) {
    return degrees * kPi / 180.0;
}

RtcpVector rotate_x(RtcpVector vector, double angle) {
    const double c = std::cos(angle);
    const double s = std::sin(angle);
    return {
        vector.x,
        vector.y * c - vector.z * s,
        vector.y * s + vector.z * c,
    };
}

RtcpVector rotate_y(RtcpVector vector, double angle) {
    const double c = std::cos(angle);
    const double s = std::sin(angle);
    return {
        vector.x * c + vector.z * s,
        vector.y,
        -vector.x * s + vector.z * c,
    };
}

RtcpVector rotate_z(RtcpVector vector, double angle) {
    const double c = std::cos(angle);
    const double s = std::sin(angle);
    return {
        vector.x * c - vector.y * s,
        vector.x * s + vector.y * c,
        vector.z,
    };
}

} // namespace

RtcpVector rtcp_rotate_abc_degrees(RtcpVector vector, double a_deg, double b_deg, double c_deg) {
    vector = rotate_x(vector, radians(a_deg));
    vector = rotate_y(vector, radians(b_deg));
    vector = rotate_z(vector, radians(c_deg));
    return vector;
}

RtcpVector rtcp_tool_vector_from_pose(const CncSimPose &pose, double tool_length) {
    return rtcp_rotate_abc_degrees({0.0, 0.0, -tool_length}, pose.a, pose.b, pose.c);
}

CncSimPose rtcp_pivot_from_tool_tip(const CncSimPose &tool_tip, double tool_length) {
    const RtcpVector tool = rtcp_tool_vector_from_pose(tool_tip, tool_length);
    CncSimPose pivot = tool_tip;
    pivot.x = tool_tip.x - tool.x;
    pivot.y = tool_tip.y - tool.y;
    pivot.z = tool_tip.z - tool.z;
    return pivot;
}

CncSimPose rtcp_tool_tip_from_pivot(const CncSimPose &pivot, double tool_length) {
    const RtcpVector tool = rtcp_tool_vector_from_pose(pivot, tool_length);
    CncSimPose tool_tip = pivot;
    tool_tip.x = pivot.x + tool.x;
    tool_tip.y = pivot.y + tool.y;
    tool_tip.z = pivot.z + tool.z;
    return tool_tip;
}