#include <cmath>
#include <iostream>

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

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

namespace {

int near(double actual, double expected)
{
    return std::fabs(actual - expected) < 1e-6;
}

int near_pose(const EmcPose &actual, const EmcPose &expected)
{
    return near(actual.tran.x, expected.tran.x) &&
           near(actual.tran.y, expected.tran.y) &&
           near(actual.tran.z, expected.tran.z) &&
           near(actual.a, expected.a) &&
           near(actual.b, expected.b);
}

int positive_struts(const double *joints)
{
    for (int index = 0; index < 5; ++index) {
        if (joints[index] <= 0.0) {
            return 0;
        }
    }
    return 1;
}

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 << "_ab="
              << pose.a << ","
              << pose.b << "\n";
}

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

}  // namespace

int main()
{
    const int init_rc = rtapi_app_main();
    std::cout << "pentakins_init=" << init_rc << "\n";
    std::cout << "pentakins_type=" << kinematicsType() << "\n";
    std::cout << "pentakins_switchable=" << kinematicsSwitchable() << "\n";

    EmcPose pose{};
    pose.tran.x = 12.5;
    pose.tran.y = -8.25;
    pose.tran.z = 25.0;
    pose.a = 2.0;
    pose.b = -1.5;

    KINEMATICS_FORWARD_FLAGS fflags = 0;
    KINEMATICS_INVERSE_FLAGS iflags = 0;
    double inverse_joints[EMCMOT_MAX_JOINTS]{};
    const int inverse_rc = kinematicsInverse(&pose, inverse_joints, &iflags, &fflags);
    std::cout << "pentakins_inverse=" << inverse_rc << "\n";
    print_joints("pentakins_inverse", inverse_joints);
    std::cout << "pentakins_positive_struts=" << positive_struts(inverse_joints) << "\n";

    EmcPose forward_pose = pose;
    const int forward_rc = kinematicsForward(inverse_joints, &forward_pose, &fflags, &iflags);
    std::cout << "pentakins_forward=" << forward_rc << "\n";
    print_pose("pentakins_forward", forward_pose);
    std::cout << "pentakins_roundtrip_pose=" << near_pose(forward_pose, pose) << "\n";

    rtapi_app_exit();
    return 0;
}
