/********************************************************************
* Description: galvo9kins.c
*   Kinematics for 2 axis galvanometer scanner
*   Derived from:
*
* Author: Jozsef Papp / hg5bsd 
* License: coding and fitting it into EMC2 from our part is GPL alike
*         
* System: realtime Linux, EMC2
* Copyright (c) 2010 All rights reserved.
* Last change:                   
*                                2011.06. 26.  only > EMC2 2.4.0
********************************************************************/
#include "kinematics.h"             /* these decls */
#include "rtapi_math.h"


 /* robot geometry  */
 /* 2 axis galvanometer scanner kinematics for slow scan cnc laser-plotter.
     d1 - tükör-tengelyek távolsága  / mirror-axis distance
     d2 - AD (joints[0]) és az ernyő távolsága / AD (joint[0] and screen distance   
     AD (joints[0]) - ernyő felöli tükör mozgató motor, y irány / screen side mirror moving motor, y direction
     CD (joints[1]) - lézer felöli tükörmozgató motor, x irány / laser side mirror moving motor, x direction


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

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

#define D1 (*(haldata->d1))    /*tükör-tengelyek távolsága*/
#define D2 (*(haldata->d2))    /*AD (joints[0]) és az ernyő távolsága*/
   

#else
double D1, D2;
#endif

#define sq(x) ((x)*(x))
#define PI 3.14159265358979323846

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

{
#define AD (joints[0])       /*ernyő felöli tükör mozgató motor, y irány*/
#define CD (joints[1])       /*lézer felöli tükörmozgató motor, x irány*/
#define Dx (pos->tran.x)
#define Dy (pos->tran.y)


   Dx=(D1+sqrt(sq(D2)+sq(Dy)))*tan(2*(CD)*PI/180);
   Dy=D2*tan(2*(AD)*PI/180);
   
/* pos->a = 0.0;
  pos->b = 0.0;
  pos->c = 0.0;*/

  return 0;

#undef AD
#undef CD
#undef Dx
#undef Dy
} 


int kinematicsInverse(const EmcPose * pos,
                      double * joints,
                      const KINEMATICS_INVERSE_FLAGS * iflags,
                      KINEMATICS_FORWARD_FLAGS * fflags)
{
#define AD (joints[0])
#define CD (joints[1])
#define Dx (pos->tran.x)
#define Dy (pos->tran.y)

   AD=0.5*atan(Dy/D2)*180/PI;
   CD=0.5*atan(Dx/(D1+sqrt(sq(D2)+sq(Dy))))*180/PI;  



  return 0;

#undef AD
#undef CD
#undef Dx
#undef Dy
}

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

#ifdef RTAPI
#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 res = 0;

    comp_id = hal_init("galvo9kins");
    if(comp_id < 0) return comp_id;

    haldata = hal_malloc(sizeof(struct haldata));
    if(!haldata) goto error;
	
    if((res = hal_pin_float_new("galvo9kins.d1", HAL_IO, &(haldata->d1), comp_id)) < 0) goto error;
    if((res = hal_pin_float_new("galvo9kins.d2", HAL_IO, &(haldata->d2), comp_id)) < 0) goto error;

    D1 = D2 = 1.0;
    
    hal_ready(comp_id);
    return 0;

error:
    hal_exit(comp_id);
    return res;
}

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