#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_joints(const double *actual, const double *expected)
{
    for (int index = 0; index < 6; ++index) {
        if (!near(actual[index], expected[index])) {
            return 0;
        }
    }
    return 1;
}

int near_identity_pose(const EmcPose &actual, const double *joints)
{
    return near(actual.tran.x, joints[0]) &&
           near(actual.tran.y, joints[1]) &&
           near(actual.tran.z, joints[2]) &&
           near(actual.a, joints[3]) &&
           near(actual.b, joints[4]) &&
           near(actual.c, joints[5]) &&
           near(actual.u, joints[6]) &&
           near(actual.v, joints[7]) &&
           near(actual.w, joints[8]);
}

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 << "_xyzabcuvw="
              << joints[0] << ","
              << joints[1] << ","
              << joints[2] << ","
              << 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 << "genser_init=" << init_rc << "\n";
    std::cout << "genser_type=" << kinematicsType() << "\n";
    std::cout << "genser_switchable=" << kinematicsSwitchable() << "\n";

    double joints[EMCMOT_MAX_JOINTS]{};
    joints[0] = 10.0;
    joints[1] = -15.0;
    joints[2] = 20.0;
    joints[3] = 5.0;
    joints[4] = -10.0;
    joints[5] = 12.0;
    joints[6] = 7.0;
    joints[7] = 8.0;
    joints[8] = 9.0;

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

    double inverse_joints[EMCMOT_MAX_JOINTS]{};
    for (int index = 0; index < EMCMOT_MAX_JOINTS; ++index) {
        inverse_joints[index] = joints[index];
    }
    const int inverse_rc = kinematicsInverse(&forward_pose, inverse_joints, &iflags, &fflags);
    std::cout << "genser_inverse=" << inverse_rc << "\n";
    print_joints("genser_inverse", inverse_joints);
    std::cout << "genser_roundtrip_joints=" << near_joints(inverse_joints, joints) << "\n";
    std::cout << "genser_roundtrip_uvw="
              << (near(inverse_joints[6], joints[6]) &&
                  near(inverse_joints[7], joints[7]) &&
                  near(inverse_joints[8], joints[8]))
              << "\n";

    const int switch_rc = kinematicsSwitch(1);
    std::cout << "genser_switch_identity=" << switch_rc << "\n";
    EmcPose identity_pose{};
    const int identity_forward_rc = kinematicsForward(joints, &identity_pose, &fflags, &iflags);
    std::cout << "genser_identity_forward=" << identity_forward_rc << "\n";
    print_pose("genser_identity", identity_pose);
    std::cout << "genser_identity_near=" << near_identity_pose(identity_pose, joints) << "\n";

    rtapi_app_exit();
    return 0;
}
