#pragma once

#include "cnc_sim_api.h"

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

struct LinuxCncFiveAxisJoints {
    double x;
    double y;
    double z;
    double a;
    double b;
    double c;
    double w;
};

struct LinuxCncAxisJoints {
    double x;
    double y;
    double z;
    double a;
    double b;
    double c;
    double u;
    double v;
    double w;
};

struct LinuxCncXyzbcTrtParameters {
    double x_rot_point;
    double y_rot_point;
    double z_rot_point;
    double x_offset;
    double y_offset;
    double z_offset;
    double tool_offset;
    bool conventional_directions;
};

struct LinuxCncScaraParameters {
    double d1;
    double d2;
    double d3;
    double d4;
    double d5;
    double d6;
};

struct LinuxCncXyzabTdrParameters {
    double tool_offset_z;
    double x_offset;
    double z_offset;
    double x_rot_point;
    double y_rot_point;
    double z_rot_point;
};

struct LinuxCncPumaParameters {
    double a2;
    double a3;
    double d3;
    double d4;
    double d6;
};

struct LinuxCncGenserParameters {
    double a[6];
    double alpha[6];
    double d[6];
    int unrotate[6];
};

struct LinuxCncScaraJoints {
    double j0;
    double j1;
    double j2;
    double j3;
    double j4;
    double j5;
};

struct LinuxCncGenhexJoints {
    double strut[6];
};

struct LinuxCncPentakinsJoints {
    double strut[5];
};

struct LinuxCncTripodJoints {
    double strut[3];
};

struct LinuxCncScorbotJoints {
    double j0;
    double j1;
    double j2;
    double j3;
    double j4;
};

struct LinuxCncLinearDeltaParameters {
    double r;
    double l;
};

struct LinuxCncRotaryDeltaParameters {
    double platformradius;
    double thighlength;
    double shinlength;
    double footradius;
};

struct LinuxCncMaxkinsParameters {
    double pivot_length;
    bool conventional_directions;
};

LinuxCncAxisJoints linuxcnc_identity_default_joints();
CncSimPose linuxcnc_identity_forward(const LinuxCncAxisJoints &joints);
LinuxCncAxisJoints linuxcnc_identity_inverse(const CncSimPose &pose);

CncSimPose linuxcnc_corexykins_forward(const LinuxCncAxisJoints &joints);
LinuxCncAxisJoints linuxcnc_corexykins_inverse(const CncSimPose &pose);

CncSimPose linuxcnc_rotatekins_forward(const LinuxCncAxisJoints &joints);
LinuxCncAxisJoints linuxcnc_rotatekins_inverse(const CncSimPose &pose);
LinuxCncGenhexJoints linuxcnc_genhex_inverse(const CncSimPose &pose);
CncSimPose linuxcnc_genhex_forward(const LinuxCncGenhexJoints &joints, const CncSimPose &initial_pose);
LinuxCncPentakinsJoints linuxcnc_pentakins_inverse(const CncSimPose &pose);
CncSimPose linuxcnc_pentakins_forward(const LinuxCncPentakinsJoints &joints,
                                      const CncSimPose &initial_pose);
LinuxCncTripodJoints linuxcnc_tripodkins_inverse(const CncSimPose &pose);
CncSimPose linuxcnc_tripodkins_forward(const LinuxCncTripodJoints &joints);
LinuxCncScorbotJoints linuxcnc_scorbot_kins_inverse(const CncSimPose &pose);
CncSimPose linuxcnc_scorbot_kins_forward(const LinuxCncScorbotJoints &joints);
LinuxCncLinearDeltaParameters linuxcnc_lineardelta_default_parameters();
CncSimPose linuxcnc_lineardelta_forward(const LinuxCncAxisJoints &joints,
                                        const LinuxCncLinearDeltaParameters &parameters);
LinuxCncAxisJoints linuxcnc_lineardelta_inverse(const CncSimPose &pose,
                                                const LinuxCncLinearDeltaParameters &parameters);
LinuxCncRotaryDeltaParameters linuxcnc_rotarydelta_default_parameters();
CncSimPose linuxcnc_rotarydelta_forward(const LinuxCncAxisJoints &joints,
                                        const LinuxCncRotaryDeltaParameters &parameters);
LinuxCncAxisJoints linuxcnc_rotarydelta_inverse(const CncSimPose &pose,
                                                const LinuxCncRotaryDeltaParameters &parameters);
LinuxCncMaxkinsParameters linuxcnc_maxkins_default_parameters();
CncSimPose linuxcnc_maxkins_forward(const LinuxCncAxisJoints &joints,
                                    const LinuxCncMaxkinsParameters &parameters);
LinuxCncAxisJoints linuxcnc_maxkins_inverse(const CncSimPose &pose,
                                            const LinuxCncMaxkinsParameters &parameters);
CncSimPose linuxcnc_rosekins_forward(const LinuxCncAxisJoints &joints);
LinuxCncAxisJoints linuxcnc_rosekins_inverse(const CncSimPose &pose);

CncSimPose linuxcnc_5axis_forward(const LinuxCncFiveAxisJoints &joints, double pivot_length);
LinuxCncFiveAxisJoints linuxcnc_5axis_inverse(const CncSimPose &pose, double pivot_length);

LinuxCncXyzbcTrtParameters linuxcnc_xyzbc_trt_default_parameters(double tool_offset);
CncSimPose linuxcnc_xyzbc_trt_forward(const LinuxCncFiveAxisJoints &joints,
                                      const LinuxCncXyzbcTrtParameters &parameters);
LinuxCncFiveAxisJoints linuxcnc_xyzbc_trt_inverse(const CncSimPose &pose,
                                                  const LinuxCncXyzbcTrtParameters &parameters);
CncSimPose linuxcnc_xyzac_trt_forward(const LinuxCncFiveAxisJoints &joints,
                                      const LinuxCncXyzbcTrtParameters &parameters);
LinuxCncFiveAxisJoints linuxcnc_xyzac_trt_inverse(const CncSimPose &pose,
                                                  const LinuxCncXyzbcTrtParameters &parameters);
CncSimPose linuxcnc_xyzbc_userk_forward(const LinuxCncFiveAxisJoints &joints);
LinuxCncFiveAxisJoints linuxcnc_xyzbc_userk_inverse(const CncSimPose &pose);

LinuxCncScaraParameters linuxcnc_scara_default_parameters();
CncSimPose linuxcnc_scara_forward(const LinuxCncScaraJoints &joints,
                                  const LinuxCncScaraParameters &parameters,
                                  int *iflags);
LinuxCncScaraJoints linuxcnc_scara_inverse(const CncSimPose &pose,
                                           const LinuxCncScaraParameters &parameters,
                                           int iflags);

LinuxCncXyzabTdrParameters linuxcnc_xyzab_tdr_default_parameters(double tool_offset_z);
CncSimPose linuxcnc_xyzab_tdr_identity_forward(const LinuxCncAxisJoints &joints);
LinuxCncAxisJoints linuxcnc_xyzab_tdr_identity_inverse(const CncSimPose &pose);
CncSimPose linuxcnc_xyzab_tdr_tcp_forward(const LinuxCncAxisJoints &joints,
                                          const LinuxCncXyzabTdrParameters &parameters);
LinuxCncAxisJoints linuxcnc_xyzab_tdr_tcp_inverse(const CncSimPose &pose,
                                                  const LinuxCncXyzabTdrParameters &parameters);

LinuxCncPumaParameters linuxcnc_puma_default_parameters();
CncSimPose linuxcnc_puma_forward(const LinuxCncAxisJoints &joints,
                                 const LinuxCncPumaParameters &parameters,
                                 int *iflags);
LinuxCncAxisJoints linuxcnc_puma_inverse(const CncSimPose &pose,
                                         const LinuxCncAxisJoints &current_joints,
                                         const LinuxCncPumaParameters &parameters,
                                         int iflags,
                                         int *fflags);

LinuxCncGenserParameters linuxcnc_genser_puma560_parameters();
LinuxCncGenserParameters linuxcnc_genser_default_parameters(int *max_iterations = nullptr);
CncSimPose linuxcnc_genser_forward(const LinuxCncAxisJoints &joints,
                                   const LinuxCncGenserParameters &parameters);
LinuxCncAxisJoints linuxcnc_genser_inverse(const CncSimPose &pose,
                                           const LinuxCncAxisJoints &joint_estimate,
                                           const LinuxCncGenserParameters &parameters,
                                           int *iterations,
                                           int max_iterations = 100);
bool linuxcnc_genser_inverse_checked(const CncSimPose &pose,
                                     const LinuxCncAxisJoints &joint_estimate,
                                     const LinuxCncGenserParameters &parameters,
                                     LinuxCncAxisJoints *joints,
                                     int *iterations,
                                     int max_iterations = 100);