/********************************************************************
    * Description: relkins.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 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_JOINTS];
        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 = "relkins";
    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_JOINTS];

    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");




    #define adj_offset_f            ((hal_float_t)_axis[axis].adj_offset)
    #define adj_steps_f             ((hal_float_t)_axis[axis].adj_steps)
    #define adj_steps_i             _axis[axis].adj_steps
    #define adj_step_size_f         ((hal_float_t)_axis[axis].adj_step_size)

    int
    kinematicsForward
    (
        const double *                      joints,
        EmcPose *                           pos,
        const KINEMATICS_FORWARD_FLAGS *    fflags,
        KINEMATICS_INVERSE_FLAGS *          iflags
    )
    {
        static hal_bit_t r0_valid, r1_valid, c0_valid, c1_valid;
        static int32_t step_0, step_1, axis, r0, r1, c0, c1;
        static double adj, adj_0, adj_1, rm, cm, r0c0_adj, r0c1_adj, r1c0_adj, r1c1_adj;
        double* axis_pos[EMCMOT_MAX_JOINTS] =
        {
            &(pos->tran.x), &(pos->tran.y), &(pos->tran.z),
            &(pos->a), &(pos->b), &(pos->c),
            &(pos->u), &(pos->v), &(pos->w)
        };

    #define joint_num               _axis[axis].joint_id[0]
    #define joint_pos               joints[joint_num]

    #define rel_axis0               _axis[axis].adj_from[0]
    #define rel_joint0_num          _axis[rel_axis0].joint_id[0] // use only 1st joint of related axis
    #define rel_joint0_pos          joints[rel_joint0_num]
    #define rel_joint0_offset_pos   (rel_joint0_pos - adj_offset_f)

    #define rel_axis1               _axis[axis].adj_from[1]
    #define rel_joint1_num          _axis[rel_axis1].joint_id[0]
    #define rel_joint1_pos          joints[rel_joint1_num]
    #define rel_joint1_offset_pos   (rel_joint1_pos - adj_offset_f)

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

            *axis_pos[axis] = joint_pos;

            if ( !_axis[axis].adj_enable ) continue;

            // line adjustment
            if ( rel_axis1 < 0 )
            {
                step_0 = (int32_t) (rel_joint0_offset_pos / adj_step_size_f);
                step_1 = step_0 + 1;

                adj_0 = step_0 >= 0 && step_0 < adj_steps_i ? *_adj[axis][step_0] : 0.0;
                adj_1 = step_1 >= 0 && step_1 < adj_steps_i ? *_adj[axis][step_1] : 0.0;

                adj = adj_0
                    + (adj_1 - adj_0)
                    * (rel_joint0_offset_pos - adj_step_size_f * ((hal_float_t)step_0))
                    / adj_step_size_f;

                *axis_pos[axis] += adj;
            }
            // plane adjustment
            else
            {
                // get row data
                r0 = (int32_t) (rel_joint0_offset_pos / adj_step_size_f);
                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) (rel_joint1_offset_pos / adj_step_size_f);
                c1 = c0 + 1;
                c0_valid = c0 >= 0 && c0 < adj_steps_i;
                c1_valid = c1 >= 0 && c1 < adj_steps_i;

                // no adjustment if current position is out of adjustment zone
    //            if ( !r0_valid && !r1_valid && !c0_valid && !c1_valid ) continue;

                // 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 = (rel_joint0_offset_pos - ((double)r0) * adj_step_size_f) / adj_step_size_f;
                cm = (rel_joint1_offset_pos - ((double)c0) * adj_step_size_f) / adj_step_size_f;
                if ( rm < 0.000001 ) rm = 0.0;
                else if ( rm > 0.999999 ) rm = 1.0;
                if ( cm < 0.000001 ) cm = 0.0;
                else if ( cm > 0.999999 ) cm = 1.0;

                adj_0 = (((r1c1_adj - r1c0_adj) * rm) - ((r0c1_adj - r0c0_adj) * rm)) * cm;
                adj_1 = (((r1c1_adj - r0c1_adj) * cm) - ((r1c0_adj - r0c0_adj) * cm)) * rm;

                adj = (adj_0 + adj_1) / 2;

                *axis_pos[axis] += adj;
            }
        }

    #undef joint_num
    #undef joint_pos

    #undef rel_axis0
    #undef rel_joint0_num
    #undef rel_joint0_pos
    #undef rel_joint0_offset_pos

    #undef rel_axis1
    #undef rel_joint1_num
    #undef rel_joint1_pos
    #undef rel_joint1_offset_pos

        return 0;
    }

    int
    kinematicsInverse
    (
        const EmcPose *                     pos,
        double *                            joints,
        const KINEMATICS_INVERSE_FLAGS *    iflags,
        KINEMATICS_FORWARD_FLAGS *          fflags
    )
    {
        static hal_bit_t r0_valid, r1_valid, c0_valid, c1_valid;
        static int32_t step_0, step_1, axis, n, r0, r1, c0, c1;
        static double adj, adj_0, adj_1, rm, cm, r0c0_adj, r0c1_adj, r1c0_adj, r1c1_adj;
        const double* axis_pos[EMCMOT_MAX_JOINTS] =
        {
            &(pos->tran.x), &(pos->tran.y), &(pos->tran.z),
            &(pos->a), &(pos->b), &(pos->c),
            &(pos->u), &(pos->v), &(pos->w)
        };

    #define joint_num               _axis[axis].joint_id[n]
    #define joint_pos               joints[joint_num]

    #define rel_axis0               _axis[axis].adj_from[0]
    #define rel_axis0_pos           *axis_pos[rel_axis0]
    #define rel_axis0_offset_pos    (rel_axis0_pos - adj_offset_f)

    #define rel_axis1               _axis[axis].adj_from[1]
    #define rel_axis1_pos           *axis_pos[rel_axis1]
    #define rel_axis1_offset_pos    (rel_axis1_pos - adj_offset_f)

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

            for ( n = _axis[axis].joint_cnt; n--; )
            {
                joint_pos = *axis_pos[axis];

                if ( !_axis[axis].adj_enable ) continue;

                // line adjustment
                if ( rel_axis1 < 0 )
                {
                    step_0 = (int32_t) (rel_axis0_offset_pos / adj_step_size_f);
                    step_1 = step_0 + 1;

                    adj_0 = step_0 >= 0 && step_0 < adj_steps_i ? *_adj[axis][step_0] : 0.0;
                    adj_1 = step_1 >= 0 && step_1 < adj_steps_i ? *_adj[axis][step_1] : 0.0;

                    adj = adj_0
                        + (adj_1 - adj_0)
                        * (rel_axis0_offset_pos - adj_step_size_f * ((hal_float_t)step_0))
                        / adj_step_size_f;

                    joint_pos -= adj;
                }
                // plane adjustment
                else
                {
                    // get row data
                    r0 = (int32_t) (rel_axis0_offset_pos / adj_step_size_f);
                    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) (rel_axis1_offset_pos / adj_step_size_f);
                    c1 = c0 + 1;
                    c0_valid = c0 >= 0 && c0 < adj_steps_i;
                    c1_valid = c1 >= 0 && c1 < adj_steps_i;

                    // no adjustment if current position is out of adjustment zone
    //                if ( !r0_valid && !r1_valid && !c0_valid && !c1_valid ) continue;

                    // 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 = (rel_axis0_offset_pos - ((double)r0) * adj_step_size_f) / adj_step_size_f;
                    cm = (rel_axis1_offset_pos - ((double)c0) * adj_step_size_f) / adj_step_size_f;
                    if ( rm < 0.000001 ) rm = 0.0;
                    else if ( rm > 0.999999 ) rm = 1.0;
                    if ( cm < 0.000001 ) cm = 0.0;
                    else if ( cm > 0.999999 ) cm = 1.0;

                    adj_0 = (((r1c1_adj - r1c0_adj) * rm) - ((r0c1_adj - r0c0_adj) * rm)) * cm;
                    adj_1 = (((r1c1_adj - r0c1_adj) * cm) - ((r1c0_adj - r0c0_adj) * cm)) * rm;

                    adj = (adj_0 + adj_1) / 2;

                    joint_pos -= adj;
                }
            }
        }

    #undef joint_num
    #undef joint_pos

    #undef rel_axis0
    #undef rel_axis0_pos
    #undef rel_axis0_offset_pos

    #undef rel_axis1
    #undef rel_axis1_pos
    #undef rel_axis1_offset_pos

        return 0;
    }

    #undef adj_offset_f
    #undef adj_steps_f
    #undef adj_steps_i
    #undef adj_step_size_f

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




    static int axis_name_to_id(char axis_char)
    {
        int axis;

        for ( axis = EMCMOT_MAX_JOINTS; 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_JOINTS] = {-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_JOINTS * 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_JOINTS; token++ )
        {
            if ( *token == ' ' || *token == '\t' || *token == ',' ) continue;

            axis = axis_name_to_id(*token);
            if ( axis >= 0 && axis < EMCMOT_MAX_JOINTS )
            {
                _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_JOINTS; token++ )
        {
            if ( *token == ' ' || *token == '\t' || *token == ',' ) continue;

            axis = axis_name_to_id(*token);
            if ( axis >= 0 && axis < EMCMOT_MAX_JOINTS && _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);
    #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);
    #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 = kstrtos32(token, 10, (int32_t*) &_axis[axis].adj_offset);
    #endif
        }

        // mem alloc, export pins
        for ( axis = EMCMOT_MAX_JOINTS; 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);
    }