#include "posemath.h"
#include "rtapi_math.h"
#include "kinematics.h"             /* decls for kinematicsForward, etc. */

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

struct mk5data_data {
    hal_float_t *d1, *d2;
} *haldata = 0;

/* key dimensions

*/

#define D1 (*(haldata->d1))
#define D2 (*(haldata->d2))



/* joint[0] and joint[1] are in degrees and joint[2] is in length units */

/* forward kinematics */

int kinematicsForward(const double * joint,
                      EmcPose * world,
                      const KINEMATICS_FORWARD_FLAGS * fflags,
                      KINEMATICS_INVERSE_FLAGS * iflags)
{
    double a0, a1;
    double x, y, a, f;

/* convert joint angles to radians for sin() and cos() */

    a0 = joint[0] * ( PM_PI / 180 );
    a1 = joint[1] * ( PM_PI / 180 );
    a = 1.57;
    f = 0.40;
/* convert angles into world coords */


	x = 250 - ((D1*sin(a-a0))+(D2*cos(180-((a-a0)+f+a1))));
	y = 600 - ((D1*cos(a-a0))+(D2*sin(180-((a-a0)+f+a1))));


    *iflags = 0;
    if (joint[1] < 90)
	*iflags = 1;

    world->tran.x = x;
    world->tran.y = y;



    return (0);
}


/* inverse kinematics */

int kinematicsInverse(const EmcPose * world,
                      double * joint,
                      const KINEMATICS_INVERSE_FLAGS * iflags,
                      KINEMATICS_FORWARD_FLAGS * fflags)
{
    double q0, q1;
    double xt, yt;
    double x, y, h, h1, s, xta;

    x = world->tran.x;
    y = world->tran.y;




    xt = 250 - x;
    yt = 600 - y;

    	h = sqrt(x*x + y*y);
	h1 = (((D1*D1)+(D2*D2))-(h*h))/2*(D1+D2);
	s =  (((D1*D1)+(h*h))-(D2*D2))/2*(D1+h);
	xta = x/y;

	q0 = s + xta;
	q1 = h1 - 0.3078163;

    q0 = q0 * (180 / PM_PI);
    q1 = q1 * (180 / PM_PI);

    joint[0] = q0;
    joint[1] = q1;



    *fflags = 0;

    return (0);
}


/* Kinematics Both */


#define DEFAULT_D1 753.77
#define DEFAULT_D2 580.32


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


EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);


int comp_id;
int rtapi_app_main(void) {
    comp_id = hal_init("mk5kins1");
    if(comp_id > 0) {
	hal_ready(comp_id);
	return 0;
    }
    return comp_id;
}

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