#pragma once

#include "cnc_sim_api.h"

struct RtcpVector {
    double x;
    double y;
    double z;
};

// Rotation order is intrinsic tool orientation A then B then C, represented as
// Rz(C) * Ry(B) * Rx(A) applied to a local tool vector.
RtcpVector rtcp_rotate_abc_degrees(RtcpVector vector, double a_deg, double b_deg, double c_deg);

RtcpVector rtcp_tool_vector_from_pose(const CncSimPose &pose, double tool_length);
CncSimPose rtcp_pivot_from_tool_tip(const CncSimPose &tool_tip, double tool_length);
CncSimPose rtcp_tool_tip_from_pivot(const CncSimPose &pivot, double tool_length);