/********************************************************************
* Description: mykins.c
*   kinematics for 2D Drawing Machine Cartesian machine
*
*   Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*    
* Copyright (c) 2004 All rights reserved.
*
* Last change: 6/23/2013
********************************************************************/

#include "rtapi_math.h"
#include "posemath.h"
#include "kinematics.h"	

#define sq(x) ((x)*(x))


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

	double c2 = 140.4225; // arm length squared
	double b = (joints[1] - joints[0]) / 2;

	
		pos->tran.x = joints[0] + b;
		pos->tran.y = sqrt(c2 - sq(b));
    	pos->tran.z = joints[2];
    	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)
{
	
	double c2 = 140.4225; // arm length squared

	double b = sqrt(c2 - sq(pos->tran.y));

	joints[0] = pos->tran.x - b;
	joints[1] = pos->tran.x + b;
	joints[2] = pos->tran.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()
{
/*
     The type of kinematics used.
    
     KINEMATICS_IDENTITY means that the joints and world coordinates are the
     same, as for slideway machines (XYZ milling machines). The EMC will allow
     changing from joint to world mode and vice versa. Also, the EMC will set
     the actual world position to be the actual joint positions (not commanded)
     by calling the forward kinematics each trajectory cycle.
   
     KINEMATICS_FORWARD_ONLY means that only the forward kinematics exist.
     Since the EMC requires at least the inverse kinematics, this should simply
     terminate the EMC.
   
     KINEMATICS_INVERSE_ONLY means that only the inverse kinematics exist.
     The forwards won't be called, and the EMC will only allow changing from
     joint to world mode at the home position.
   
     KINEMATICS_BOTH means that both the forward and inverse kins are defined.
     Like KINEMATICS_IDENTITY, the EMC will allow changing between world and
     joint modes. However, the kins are assumed to be somewhat expensive
     computationally, and the forwards won't be called at the trajectory rate
     to compute actual world coordinates from actual joint values.
*/

    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) {
    comp_id = hal_init("mykins");
    if(comp_id > 0) {
	hal_ready(comp_id);
	return 0;
    }
    return comp_id;
}

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