#include "kinematics.h"		/* these decls */
#include "posemath.h"
#include "hal.h"
#include "rtapi.h"
#include "rtapi_math.h"

#define d2r(d) ((d)*PM_PI/180.0)
#define r2d(r) ((r)*180.0/PM_PI)

struct haldata {
    hal_float_t *tool_length;
    hal_float_t *pivot_length;

} *haldata;

static PmCartesian s2r(double r, double t, double p ) {
    PmCartesian c;

    double dx = -0.63;
    double dy = -0.04;


    t = d2r(t), p = d2r(p);

    double t1 = sqrt(dx*dx*cos(p)*cos(p)+dy*dy);
    double t2 = t + atan(dy/dx/cos(p));

    c.x = t1*cos(t2) - r * sin(p)*cos(t);
    c.y = t1*sin(t2) - r * sin(p)*sin(t);
    c.z = -(dx*sin(p)+r*cos(p));



    return c;
}

int kinematicsForward(const double *joints,
		      EmcPose * pos,
		      const KINEMATICS_FORWARD_FLAGS * fflags,
		      KINEMATICS_INVERSE_FLAGS * iflags)
{
    PmCartesian r = s2r(*(haldata->pivot_length)+*(haldata->tool_length), joints[5], joints[4]);

    pos->tran.x = joints[0] + r.x;
    pos->tran.y = joints[1] + r.y;
    pos->tran.z = joints[2] + r.z;
    pos->a = joints[3];
    pos->b = joints[4];
    pos->c = joints[5];
    pos->u = joints[6];
    pos->v = joints[7];
    pos->w = joints[8];

    return 0;
}

int kinematicsInverse(const EmcPose * pos,
		      double *joints,
		      const KINEMATICS_INVERSE_FLAGS * iflags,
		      KINEMATICS_FORWARD_FLAGS * fflags)
{

    PmCartesian r = s2r(*(haldata->pivot_length)+*(haldata->tool_length), pos->c, pos->b);

    joints[0] = pos->tran.x - r.x;
    joints[1] = pos->tran.y - r.y;
    joints[2] = pos->tran.z - r.z;
    joints[3] = pos->a;
    joints[4] = pos->b;
    joints[5] = pos->c;
    joints[6] = pos->u;
    joints[7] = pos->v;
    joints[8] = pos->w;

    return 0;
}

/* implemented for these kinematics as giving joints preference */
int kinematicsHome(EmcPose * world,
		   double *joint,
		   KINEMATICS_FORWARD_FLAGS * fflags,
		   KINEMATICS_INVERSE_FLAGS * iflags)
{
    *fflags = 0;
    *iflags = 0;

    return kinematicsForward(joint, world, fflags, iflags);
}

KINEMATICS_TYPE kinematicsType()
{
    return KINEMATICS_BOTH;
}

#include "rtapi.h"		/* RTAPI realtime OS API */
#include "rtapi_app.h"		/* RTAPI realtime module decls */
#include "hal.h"

EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
MODULE_LICENSE("GPL");

int comp_id;
int rtapi_app_main(void) {
    int result;
    comp_id = hal_init("5axecomp");
    if(comp_id < 0) return comp_id;

    haldata = hal_malloc(sizeof(struct haldata));

    result = hal_pin_float_new("5axe.tooloffset.z", HAL_IN, &(haldata->tool_length), comp_id);
    if(result < 0) goto error;
    result = hal_pin_float_new("5axe.pivot-length", HAL_IO, &(haldata->pivot_length), comp_id);
    if(result < 0) goto error;

    *(haldata->pivot_length) = 185.455;

    hal_ready(comp_id);
    return 0;

error:
    hal_exit(comp_id);
    return result;
}

void rtapi_app_exit(void) { hal_exit(comp_id); }