/********************************************************************
* Description: maxkins.c
*   Kinematics for Chris Radek's tabletop 5 axis mill named 'max'.
*   This mill has a tilting head (B axis) and horizontal rotary
*   mounted to the table (C axis).
*
* Author: Chris Radek
* License: GPL Version 2
*    
* Copyright (c) 2007 Chris Radek
********************************************************************/

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

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


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

struct haldata {
    hal_float_t *pivot_length;
} *haldata;

#define VTVERSION VTKINEMATICS_VERSION1

MODULE_LICENSE("GPL");

int kinematicsForward(const double *joints,
		      EmcPose * pos,
		      const KINEMATICS_FORWARD_FLAGS * fflags,
		      KINEMATICS_INVERSE_FLAGS * iflags)
{
    // B correction
    double zb = (*(haldata->pivot_length) + joints[8]) * rtapi_cos(d2r(joints[4]));
    double xb = (*(haldata->pivot_length) + joints[8]) * rtapi_sin(d2r(joints[4]));

    // C correction
    double xyr = rtapi_hypot(joints[0], joints[1]);
    double xytheta = rtapi_atan2(joints[1], joints[0]) + d2r(joints[5]);

    // U correction
    double zv = joints[6] * rtapi_sin(d2r(joints[4]));
    double xv = joints[6] * rtapi_cos(d2r(joints[4]));

    // V correction is always in joint 1 only

    pos->tran.x = xyr * rtapi_cos(xytheta) + xb - xv;
    pos->tran.y = xyr * rtapi_sin(xytheta) - joints[7];
    pos->tran.z = joints[2] - zb + zv + *(haldata->pivot_length);

    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)
{
    // B correction
    double zb = (*(haldata->pivot_length) + pos->w) * rtapi_cos(d2r(pos->b));
    double xb = (*(haldata->pivot_length) + pos->w) * rtapi_sin(d2r(pos->b));

    // C correction
    double xyr = rtapi_hypot(pos->tran.x, pos->tran.y);
    double xytheta = rtapi_atan2(pos->tran.y, pos->tran.x) - d2r(pos->c);

    // U correction
    double zv = pos->u * rtapi_sin(d2r(pos->b));
    double xv = pos->u * rtapi_cos(d2r(pos->b));

    // V correction is always in joint 1 only

    joints[0] = xyr * rtapi_cos(xytheta) - xb + xv;
    joints[1] = xyr * rtapi_sin(xytheta) + pos->v;
    joints[2] = pos->tran.z + zb + zv - *(haldata->pivot_length);

    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;
}

KINEMATICS_TYPE kinematicsType(void)
{
    return KINEMATICS_BOTH;
}


static vtkins_t vtk = {
    .kinematicsForward = kinematicsForward,
    .kinematicsInverse  = kinematicsInverse,
    // .kinematicsHome = kinematicsHome,
    .kinematicsType = kinematicsType
};

static int comp_id, vtable_id;
static const char *name = "maxkins";

int rtapi_app_main(void) {
    int result;
    comp_id = hal_init(name);
    if(comp_id < 0) return comp_id;

    vtable_id = hal_export_vtable(name, VTVERSION, &vtk, comp_id);
    if (vtable_id < 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
			"%s: ERROR: hal_export_vtable(%s,%d,%p) failed: %d\n",
			name, name, VTVERSION, &vtk, vtable_id );
	return -ENOENT;
    }

    haldata = hal_malloc(sizeof(struct haldata));

    result = hal_pin_float_new("maxkins.pivot-length", HAL_IO, &(haldata->pivot_length), comp_id);
    if(result < 0) goto error;

    *(haldata->pivot_length) = 0.666;

    hal_ready(comp_id);
    return 0;

error:
    hal_exit(comp_id);
    return result;
}

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