#include <iostream>

#include "emc/kinematics/kinematics.h"
#include "emc/motion/emcmotcfg.h"

int rtapi_app_main(void);
void rtapi_app_exit(void);

namespace {

void print_pose(const char *prefix, const EmcPose &pose)
{
    std::cout << prefix << "_xyz="
              << pose.tran.x << ","
              << pose.tran.y << ","
              << pose.tran.z << "\n";
    std::cout << prefix << "_abcuvw="
              << pose.a << ","
              << pose.b << ","
              << pose.c << ","
              << pose.u << ","
              << pose.v << ","
              << pose.w << "\n";
}

void print_joints(const char *prefix, const double *joints)
{
    std::cout << prefix << "_xyz="
              << joints[0] << ","
              << joints[1] << ","
              << joints[2] << "\n";
    std::cout << prefix << "_abcuvw="
              << joints[3] << ","
              << joints[4] << ","
              << joints[5] << ","
              << joints[6] << ","
              << joints[7] << ","
              << joints[8] << "\n";
}

}  // namespace

int main()
{
    const int init_rc = rtapi_app_main();
    std::cout << "kinematics_init=" << init_rc << "\n";
    std::cout << "kinematics_type=" << kinematicsType() << "\n";
    std::cout << "kinematics_switchable=" << kinematicsSwitchable() << "\n";
    std::cout << "kinematics_switch=" << kinematicsSwitch(0) << "\n";

    double joints[EMCMOT_MAX_JOINTS]{};
    for (int index = 0; index < 9; ++index) {
        joints[index] = index + 1;
    }

    EmcPose forward_pose{};
    KINEMATICS_FORWARD_FLAGS fflags = 0;
    KINEMATICS_INVERSE_FLAGS iflags = 0;
    const int forward_rc = kinematicsForward(joints, &forward_pose, &fflags, &iflags);
    std::cout << "kinematics_forward=" << forward_rc << "\n";
    print_pose("kinematics_forward", forward_pose);

    EmcPose inverse_pose{};
    inverse_pose.tran.x = 10.0;
    inverse_pose.tran.y = 20.0;
    inverse_pose.tran.z = 30.0;
    inverse_pose.a = 40.0;
    inverse_pose.b = 50.0;
    inverse_pose.c = 60.0;
    inverse_pose.u = 70.0;
    inverse_pose.v = 80.0;
    inverse_pose.w = 90.0;

    double inverse_joints[EMCMOT_MAX_JOINTS]{};
    const int inverse_rc = kinematicsInverse(&inverse_pose, inverse_joints, &iflags, &fflags);
    std::cout << "kinematics_inverse=" << inverse_rc << "\n";
    print_joints("kinematics_inverse", inverse_joints);

    rtapi_app_exit();
    return 0;
}
