/********************************************************************
* Description: krivkins.c
*
*   Trivial kinematics for 3 axis Cartesian machine
*   with position adjustment
*
* Author: Mikhail Vydrenko (mikhail@vydrenko.ru)
********************************************************************/

#define FOR_MACHINEKIT 0

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

#if FOR_MACHINEKIT
#include <stdlib.h>
#else
#include "rtapi_math.h"
#include "rtapi_string.h"
#include <linux/kernel.h>
#endif




#define ADJ_STEPS_MAX   255
#define ADJ_FORWARD     0
#define ADJ_INVERSE     1

#define PRINT_ERROR(MSG) \
    rtapi_print_msg(RTAPI_MSG_ERR, "%s: "MSG"\n", comp_name)

#define PRINT_ERROR_AND_RETURN(MSG,RETVAL) \
    { PRINT_ERROR(MSG); return RETVAL; }

#if FOR_MACHINEKIT
#define VTVERSION VTKINEMATICS_VERSION1
#endif

MODULE_AUTHOR("Mikhail Vydrenko");
MODULE_DESCRIPTION("Trivial kinematics with position adjustment");
MODULE_LICENSE("GPL");

typedef struct
{
    hal_bit_t enable;
    hal_u32_t joint_id[EMCMOT_MAX_AXIS];
    hal_u32_t joint_cnt;

    hal_bit_t adj_enable;
    hal_s32_t adj_from[2];
    hal_s32_t adj_offset;
    hal_u32_t adj_steps;
    hal_u32_t adj_step_size;
} axis_t;




#if FOR_MACHINEKIT
static int vtable_id;
#endif
static int comp_id;
static const char *comp_name = "krivkins";
static const char *axis_name0 = "xyzabcuvw";
static const char *axis_name1 = "XYZABCUVW";
static KINEMATICS_TYPE ktype = -1;
static axis_t * _axis;
static hal_float_t ** _adj[EMCMOT_MAX_AXIS];

static char *coordinates = "XYZABCUVW";
RTAPI_MP_STRING(coordinates, "Existing Axes");
static char *kinstype = "1"; // use KINEMATICS_IDENTITY
RTAPI_MP_STRING(kinstype, "Kinematics Type (Identity,Both)");
static char *adjust = "";
RTAPI_MP_STRING(adjust, "adjust axis name/ID, comma separated");
static char *from = "";
RTAPI_MP_STRING(from, "related axes names/IDs, comma separated");
static char *offset = "";
RTAPI_MP_STRING(offset, "adjust from position offset in units, comma separated");
static char *steps = "";
RTAPI_MP_STRING(steps, "steps count, comma separated");
static char *step_size = "";
RTAPI_MP_STRING(step_size, "step size in units, comma separated");




double get_adj(uint8_t axis, double src_pos0, double src_pos1);




int
kinematicsForward
(
    const double *                      joints,
    EmcPose *                           pos,
    const KINEMATICS_FORWARD_FLAGS *    fflags,
    KINEMATICS_INVERSE_FLAGS *          iflags
)
{
    static int32_t axis;
    double* axis_pos[EMCMOT_MAX_AXIS] =
    {
        &(pos->tran.x), &(pos->tran.y), &(pos->tran.z),
        &(pos->a), &(pos->b), &(pos->c),
        &(pos->u), &(pos->v), &(pos->w)
    };

    for ( axis = EMCMOT_MAX_AXIS; axis--; )
    {
        if ( !_axis[axis].enable ) continue;

        *axis_pos[axis] = joints[ _axis[axis].joint_id[0] ];
#if ADJ_FORWARD
        if ( !_axis[axis].adj_enable ) continue;

        if ( _axis[axis].adj_from[1] < 0 )
        {
            *axis_pos[axis] += get_adj(
                axis,
                joints[ _axis[ _axis[axis].adj_from[0] ].joint_id[0] ],
                0.0);
        }
        else
        {
            *axis_pos[axis] += get_adj(
                axis,
                joints[ _axis[ _axis[axis].adj_from[0] ].joint_id[0] ],
                joints[ _axis[ _axis[axis].adj_from[1] ].joint_id[0] ]);
        }
#endif
    }

    return 0;
}

int
kinematicsInverse
(
    const EmcPose *                     pos,
    double *                            joints,
    const KINEMATICS_INVERSE_FLAGS *    iflags,
    KINEMATICS_FORWARD_FLAGS *          fflags
)
{
    static int32_t axis, n;
    const double* axis_pos[EMCMOT_MAX_AXIS] =
    {
        &(pos->tran.x), &(pos->tran.y), &(pos->tran.z),
        &(pos->a), &(pos->b), &(pos->c),
        &(pos->u), &(pos->v), &(pos->w)
    };

    for ( axis = EMCMOT_MAX_AXIS; axis--; )
    {
        if ( !_axis[axis].enable ) continue;

        for ( n = _axis[axis].joint_cnt; n--; )
        {
            joints[ _axis[axis].joint_id[n] ] = *axis_pos[axis];
#if ADJ_INVERSE
            if ( !_axis[axis].adj_enable ) continue;

            if ( _axis[axis].adj_from[1] < 0 )
            {
                joints[ _axis[axis].joint_id[n] ] -= get_adj(
                    axis,
                    *axis_pos[ _axis[axis].adj_from[0] ],
                    0.0);
            }
            else
            {
                joints[ _axis[axis].joint_id[n] ] -= get_adj(
                    axis,
                    *axis_pos[ _axis[axis].adj_from[0] ],
                    *axis_pos[ _axis[axis].adj_from[1] ]);
            }
#endif
        }
    }

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

#if FOR_MACHINEKIT
static vtkins_t vtk =
{
    .kinematicsForward = kinematicsForward,
    .kinematicsInverse  = kinematicsInverse,
//  .kinematicsHome = kinematicsHome,
    .kinematicsType = kinematicsType
};
#else
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
#endif




#define adj_offset_i            _axis[axis].adj_offset
#define adj_steps_i             _axis[axis].adj_steps
#define adj_step_size_i         _axis[axis].adj_step_size
#define rel_axis0               _axis[axis].adj_from[0]
#define rel_axis1               _axis[axis].adj_from[1]

double get_adj(uint8_t axis, double src_pos0, double src_pos1)
{
    static hal_bit_t r0_valid, r1_valid, c0_valid, c1_valid;
    static int32_t r0, r1, c0, c1;
    static double adj, adj0, adj1, pos0, pos1;
    static double rm, cm, r0c0_adj, r0c1_adj, r1c0_adj, r1c1_adj;

    pos0 = src_pos0 - _axis[axis].adj_offset;

    // line adjustment
    if ( rel_axis1 < 0 )
    {
        r0 = (int32_t) (pos0 / adj_step_size_i);
        if ( pos0 < 0 ) r0--;
        r1 = r0 + 1;

        adj0 = r0 >= 0 && r0 < adj_steps_i ? *_adj[axis][r0] : 0.0;
        adj1 = r1 >= 0 && r1 < adj_steps_i ? *_adj[axis][r1] : 0.0;

        adj = adj0 + (adj1 - adj0) * (pos0 - adj_step_size_i * r0) / adj_step_size_i;
    }
    // plane adjustment
    else
    {
        pos1 = src_pos1 - _axis[axis].adj_offset;

        // get row data
        r0 = (int32_t) (pos0 / adj_step_size_i);
        if ( pos0 < 0 ) r0--;
        r1 = r0 + 1;
        r0_valid = r0 >= 0 && r0 < adj_steps_i;
        r1_valid = r1 >= 0 && r1 < adj_steps_i;

        // get column data
        c0 = (int32_t) (pos1 / adj_step_size_i);
        if ( pos1 < 0 ) c0--;
        c1 = c0 + 1;
        c0_valid = c0 >= 0 && c0 < adj_steps_i;
        c1_valid = c1 >= 0 && c1 < adj_steps_i;

        // get values of 4 adjustment cells around current position
        r0c0_adj = r0_valid && c0_valid ? *_adj[axis][r0*_axis[axis].adj_steps + c0] : 0.0;
        r0c1_adj = r0_valid && c1_valid ? *_adj[axis][r0*_axis[axis].adj_steps + c1] : 0.0;
        r1c0_adj = r1_valid && c0_valid ? *_adj[axis][r1*_axis[axis].adj_steps + c0] : 0.0;
        r1c1_adj = r1_valid && c1_valid ? *_adj[axis][r1*_axis[axis].adj_steps + c1] : 0.0;

        // calculate row/column multipliers
        rm = (pos0 - r0 * adj_step_size_i) / adj_step_size_i;
        cm = (pos1 - c0 * adj_step_size_i) / adj_step_size_i;

        adj0 = (r0c0_adj + (r0c1_adj - r0c0_adj) * cm)
             + (   (r1c0_adj + (r1c1_adj - r1c0_adj) * cm)
                 - (r0c0_adj + (r0c1_adj - r0c0_adj) * cm) ) * rm;
        adj1 = (r0c0_adj + (r1c0_adj - r0c0_adj) * rm)
             + (   (r0c1_adj + (r1c1_adj - r0c1_adj) * rm)
                 - (r0c0_adj + (r1c0_adj - r0c0_adj) * rm) ) * cm;

        adj = (adj0 + adj1) / 2;
    }

    return adj;
}




static int axis_name_to_id(char axis_char)
{
    int axis;

    for ( axis = EMCMOT_MAX_AXIS; axis--; )
    {
        if ( axis_char == axis_name0[axis] || axis_char == axis_name1[axis] ) return axis;
    }

    return -1;
}




int rtapi_app_main(void)
{
    int32_t retval, axis, joint, id, axis_list[EMCMOT_MAX_AXIS] = {-1};
    int32_t r, c;
    char *data, *token;

    // component init
    comp_id = hal_init(comp_name);
    if ( comp_id < 0 ) return -1;

#if FOR_MACHINEKIT
    // export kinematics table
    vtable_id = hal_export_vtable(comp_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",
            comp_name, comp_name, VTVERSION, &vtk, vtable_id);
        return -1;
    }
#endif

    // shared memory allocation
    _axis = hal_malloc(EMCMOT_MAX_AXIS * sizeof(axis_t));
    if ( !_axis ) PRINT_ERROR_AND_RETURN("hal_malloc() failed",-1);

    // parse `kinstype`
    switch (*kinstype)
    {
      case 'b': case 'B': ktype = KINEMATICS_BOTH;         break;
      case 'f': case 'F': ktype = KINEMATICS_FORWARD_ONLY; break;
      case 'i': case 'I': ktype = KINEMATICS_INVERSE_ONLY; break;
      case '1': default:  ktype = KINEMATICS_IDENTITY;
    }

    // parse `coordinates`
    for ( joint = 0, token = coordinates; *token && joint < EMCMOT_MAX_AXIS; token++ )
    {
        if ( *token == ' ' || *token == '\t' || *token == ',' ) continue;

        axis = axis_name_to_id(*token);
        if ( axis >= 0 && axis < EMCMOT_MAX_AXIS )
        {
            _axis[axis].enable = 1;
            _axis[axis].joint_id[ _axis[axis].joint_cnt++ ] = joint;
        }

        joint++;
    }

    // parse `adjust`
    for ( id = 0, token = adjust; *token && id < EMCMOT_MAX_AXIS; token++ )
    {
        if ( *token == ' ' || *token == '\t' || *token == ',' ) continue;

        axis = axis_name_to_id(*token);
        if ( axis >= 0 && axis < EMCMOT_MAX_AXIS && _axis[axis].enable )
        {
            _axis[axis].adj_enable = 1;
            axis_list[id] = axis;
        }

        id++;
    }

    // parse `from`
#if FOR_MACHINEKIT
    for ( id = 0, data = from; (token = strtok(data, ",")) != NULL; id++ )
    {
        if ( data != NULL ) data = NULL;
#else
    for ( id = 0, data = from; (token = strsep(&data, ",")) != NULL; id++ )
    {
#endif
        axis = axis_list[id];
        if ( axis < 0 || !_axis[axis].adj_enable ) break;

        _axis[axis].adj_from[0] = axis_name_to_id(token[0]);
        _axis[axis].adj_from[1] = axis_name_to_id(token[1]);

        if ( _axis[axis].adj_from[0] < 0 ) _axis[axis].adj_enable = 0;
    }

    // parse `steps`
#if FOR_MACHINEKIT
    for ( id = 0, data = steps; (token = strtok(data, ",")) != NULL; id++ )
    {
        if ( data != NULL ) data = NULL;
#else
    for ( id = 0, data = steps; (token = strsep(&data, ",")) != NULL; id++ )
    {
#endif
        axis = axis_list[id];
        if ( axis < 0 || !_axis[axis].adj_enable ) break;

#if FOR_MACHINEKIT
        _axis[axis].adj_steps = (hal_u32_t) strtoul(token, NULL, 10);
#else
        //retval = kstrtou32(token, 10, (uint32_t*) &_axis[axis].adj_steps);
        _axis[axis].adj_steps = (hal_u32_t) simple_strtol(token, NULL, 10);
#endif
        if ( !_axis[axis].adj_steps ) _axis[axis].adj_enable = 0;
        if ( _axis[axis].adj_steps > ADJ_STEPS_MAX ) _axis[axis].adj_steps = ADJ_STEPS_MAX;
    }

    // parse `step_size`
#if FOR_MACHINEKIT
    for ( id = 0, data = step_size; (token = strtok(data, ",")) != NULL; id++ )
    {
        if ( data != NULL ) data = NULL;
#else
    for ( id = 0, data = step_size; (token = strsep(&data, ",")) != NULL; id++ )
    {
#endif
        axis = axis_list[id];
        if ( axis < 0 || !_axis[axis].adj_enable ) break;

#if FOR_MACHINEKIT
        _axis[axis].adj_step_size = (hal_u32_t) strtoul(token, NULL, 10);
#else
        //retval = kstrtou32(token, 10, (uint32_t*) &_axis[axis].adj_step_size);
        _axis[axis].adj_step_size = (hal_u32_t) simple_strtol(token, NULL, 10);
#endif
        if ( !_axis[axis].adj_step_size ) _axis[axis].adj_enable = 0;
    }

    // parse `offset`
#if FOR_MACHINEKIT
    for ( id = 0, data = offset; (token = strtok(data, ",")) != NULL; id++ )
    {
        if ( data != NULL ) data = NULL;
#else
    for ( id = 0, data = offset; (token = strsep(&data, ",")) != NULL; id++ )
    {
#endif
        axis = axis_list[id];
        if ( axis < 0 || !_axis[axis].adj_enable ) break;

#if FOR_MACHINEKIT
        _axis[axis].adj_offset = (hal_s32_t) strtol(token, NULL, 10);
#else
        //retval = simple_strtol(token, 10, (int32_t*) &_axis[axis].adj_offset);
        _axis[axis].adj_offset = (hal_s32_t) simple_strtol(token, NULL, 10);
#endif
    }

    // mem alloc, export pins
    for ( axis = EMCMOT_MAX_AXIS; axis--; )
    {
        if ( !_axis[axis].adj_enable ) continue;

        // plane adjustment
        if ( _axis[axis].adj_from[1] >= 0 )
        {
            // mem alloc
            _adj[axis] = hal_malloc(_axis[axis].adj_steps * _axis[axis].adj_steps * sizeof(hal_float_t*));
            if ( !_adj[axis] ) PRINT_ERROR_AND_RETURN("hal_malloc() failed",-1);

            // HAL pins export
            for ( r = 0, retval = 0; r < _axis[axis].adj_steps; r++ )
            {
                for ( c = 0; c < _axis[axis].adj_steps; c++ )
                {
                    retval += hal_pin_float_newf( HAL_IN,
                        &(_adj[axis][r * _axis[axis].adj_steps + c]), comp_id,
                        "%s.adj%c.%c%d_%c%d", comp_name, axis_name1[axis],
                        axis_name1[ _axis[axis].adj_from[0] ],
                        (r * _axis[axis].adj_step_size + _axis[axis].adj_offset),
                        axis_name1[ _axis[axis].adj_from[1] ],
                        (c * _axis[axis].adj_step_size + _axis[axis].adj_offset)
                    );
                }
            }
            if ( retval ) PRINT_ERROR_AND_RETURN("HAL plane step pins export failed",-1);
        }
        // line adjustment
        else
        {
            // mem alloc
            _adj[axis] = hal_malloc(_axis[axis].adj_steps * sizeof(hal_float_t*));
            if ( !_adj[axis] ) PRINT_ERROR_AND_RETURN("hal_malloc() failed",-1);

            // HAL pins export
            for ( r = 0, retval = 0; r < _axis[axis].adj_steps; r++ )
            {
                retval += hal_pin_float_newf( HAL_IN, &_adj[axis][r], comp_id,
                    "%s.adj%c.%c%d", comp_name, axis_name1[axis],
                    axis_name1[ _axis[axis].adj_from[0] ],
                    (r * _axis[axis].adj_step_size + _axis[axis].adj_offset) );
            }
            if ( retval ) PRINT_ERROR_AND_RETURN("HAL line step pins export failed",-1);
        }
    }

    hal_ready(comp_id);
    return 0;
}




void rtapi_app_exit(void)
{
#if FOR_MACHINEKIT
    hal_remove_vtable(vtable_id);
#endif
    hal_exit(comp_id);
}