component moveoff "Component to support (Hal-only) offset moves when a program is paused.  Three axes are supported (labeled x,y,z).\n\nThe axis offset pin values ([xyz]-offset-in) are continuously applied (respecting limits on value, velocity, and acceleration) when a program is paused if move-enable is True.  The offset(s) should be returned to zero value by changing the [xyz]-offset-in pin values or by deasserting move-enable BEFORE the program is resumed.  The applied offsets are automatically returned to zero (respecting limits) when the move-enable input is deactivated.  The zero value tolerance is specified by the epsilon input pin value.\n\nNOTE: The auto-return move is NOT coordinated, each axis moves at its own rate.  If a controlled path is wanted, each axis should be manually returned to zero before deasserting the move-enable pin\n\nNOTE: The offsets-applied output pin is provided to indicate to a GUI that a program may be resumed.  If the offset(s) are non-zero when the program is resumed, the offsets are returned to zero (respecting limits) and an error message is issued.\n\nBEWARE: If a running program: 1) is paused and 2) offset(s) are applied and 3) the is-on input is deactivated (for any reason), a servo system will potentially have a large error due to the offset output position as measured by its encoder.  When the machine is subsequently turned on again, the servo will likely correct the error with full force.  YOU HAVE BEEN WARNED.\n\nNOTE: This Hal-only means of offsetting is typically not known to LinuxCNC nor available in GUI preview displays.  No protection is provided for offset moves that exeed soft limits managed by linuxCNC.  Since soft limits are not honored, an offset move may move into hard limits (or crash if there is no limit switch).  Triggering a hard limit will turn off the machine deasserting is-on -- see BEWARE above.\n\nNOTE: The [xyz]-offset-in values may be set with inifile settings or controlled by a GUI.  Fixed values may be appropriate in simple cases where the direction and amount of offset is well-defined but a control method is required to deactivate the move-enble input in order to resume the program properly.  It is recommended that GUIs set [xyz]-offset-in values to zero when not in use (e.g., when a program is running and not paused) and provide means for users to set/increment/modify/view the offset values for each axis in use.  It may be useful to provide controls to manage the offset limits on value, velocity and acceleration.\n\nNOTE: Use of the names= option for naming is recommended (e.g., loadrt moveoff names=name1,name2,...";

/*
Copyright: 2014
Author:    Dewey Garrett <dgarrett@panix.com>

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

pin in  bit move_enable "Enable offsets when is-paused and is-on";
pin in  bit is_on "Connect to halui.machine.is-on";
pin in  bit is_paused "Connect to halui.program.is-paused";
pin in float epsilon "When is-paused is deactivated, return to unpaused position within epsilon units. WARNING: Too small value may cause overshoot";
pin out bit offset_applied "True if one or more offsets are applied";

pin in float offset-in-#[9 : personality] "Joint offset value (from gui typically for dynamic use, from inifile for fixed value)";
pin in float pos-#[9 : personality] "Joint position (typ: axis.0.motor-pos-cmd)";
pin out float plusoffset-#[9 : personality] "computed joint position (plus offset while paused) (typically connect to pid-command)";
pin in float fb-#[9 : personality] "Joint feedback (typ from encoder and input to pid controller (pid.feedback))";
pin out float fb-minusoffset-#[9 : personality] "computed joint feedback (minus x offset while paused) (typically connected to axis.0.motor-pos-fb";
pin in  float offset-vel#[9 : personality] "Joint offset velocity limit";
pin in  float offset-accel-#[9 : personality] "joint offset acceleration limit";
pin out float offset-current-#[9 : personality]"joint offset current value";
pin in float offset-min-#[9 : personality] = -1e20 "minimum limit for applied joint offset (typ negative)";
pin in float offset-max-#[9 : personality] = 1e20 "maximum limit for applied x offset (typ positive)";

option data old_values_t;

option personality;
option extra_setup;
function _ ;

license "GPL";
;;

#include "rtapi_math.h"

typedef enum {
  IDLE,
  MOVE_AWAY,
  MOVE_BACK,
} the_state;

typedef struct {
    hal_float_t old_in[9];
    hal_float_t old_out[9];
    hal_float_t old_v[9];
    hal_float_t old_lin[9];
    bool        was_paused;
    bool        gave_msg;
} old_values_t;

struct lim3_input {
    hal_float_t  minlimit;
    hal_float_t  maxlimit;
    hal_float_t  maxvlimit;
    hal_float_t  maxalimit;
    hal_float_t  in;
    hal_float_t  old_in;
    hal_float_t  old_out;
    hal_float_t  old_v;
    hal_float_t  lin;
};

static long theperiod;

void reset_old(old_values_t d){
    int i;
    for (i = 0;i < 9; i++){
        d.old_in[i] = 0;
        d.old_out[i] = 0;
        d.old_v[i] = 0;
        d.old_lin[i] = 0;
    }
}

int offset_removed(old_values_t d, double eps){
    int val = TRUE;
    int i;
    for (i = 0 ; i < 9 ; i++){
        if (fabs(d.old_out[i]) > eps) val = FALSE;
    }
    return val;
}

void lim3(struct lim3_input input,double* old_in,double* lout,double* old_v) {
/* from limit3.comp */
    hal_float_t dt = theperiod * 0.000000001;
    hal_float_t in_v, min_v, max_v, avg_v;
    hal_float_t min_out,max_out;
    hal_float_t ramp_a, match_time, est_in, est_out;
    hal_float_t err, dv, dp;
    hal_float_t thelin;

    /* apply first order limit */
    thelin = input.in;
    if (thelin < input.minlimit) {
        thelin = input.minlimit;
    }
    if (thelin > input.maxlimit) {
        thelin = input.maxlimit;
    }
    *old_in = thelin;

    /* calculate input derivative */
    in_v = (thelin - input.old_in) / dt;

    /* determine v and out that can be reached in one period */
    min_v = input.old_v - input.maxalimit * dt;
    if (min_v < -input.maxvlimit) {
        min_v = -input.maxvlimit;
    }
    max_v = input.old_v + input.maxalimit * dt;
    if (max_v > input.maxvlimit) {
        max_v = input.maxvlimit;
    }

    min_out = input.old_out + min_v * dt;
    max_out = input.old_out + max_v * dt;
    if (   ( thelin >= min_out ) && ( thelin <= max_out )
        && ( in_v >= min_v ) && ( in_v <= max_v ) ) {
        /* we can follow the command without hitting a limit */
        *lout = thelin;
        input.old_v = ( *lout - input.old_out ) / dt;
    } else {
        /* can't follow commanded path while obeying limits */
        /* determine which way we need to ramp to match v */
        if ( in_v > input.old_v ) {
            ramp_a = input.maxalimit;
        } else {
            ramp_a = -input.maxalimit;
        }
        /* determine how long the match would take */
        match_time = ( in_v - input.old_v ) / ramp_a;
        /* where we will be at the end of the match */

        avg_v = ( in_v + input.old_v + ramp_a * dt ) * 0.5;
        est_out = input.old_out + avg_v * match_time;
        /* calculate the expected command position at that time */
        est_in = input.old_in + in_v * match_time;
        /* calculate position error at that time */
        err = est_out - est_in;
        /* calculate change in final position if we ramp in the
        opposite direction for one period */
        dv = -2.0 * ramp_a * dt;
        dp = dv * match_time;
        /* decide what to do */
        if ( fabs(err + dp*2.0) < fabs(err) ) {
            ramp_a = -ramp_a;
        }
        if ( ramp_a < 0.0 ) {
            *lout = min_out;
            *old_v = min_v;
        } else {
            *lout = max_out;
            *old_v = max_v;
        }
    }
    return;
}

static struct lim3_input input;
static the_state state = IDLE;

FUNCTION(_) {
    double lout,old_in,old_v;
    bool enable  =  move_enable && is_on && is_paused;
    bool move = 0;
    int i;

    theperiod = period;

    for (i = 0; i < personality ; i++){
        input.in = offset_in(i);
        switch (state) {
          case IDLE:
               data.was_paused = 0;
               data.gave_msg = 0;
               if ( enable ) {
                    state = MOVE_AWAY;
                    move = 1;
                }
                break;
          case MOVE_AWAY:
                data.was_paused = 0;
                if (enable) {
                   // allow offset movements
                   move = 1;
                   break;
                }
                // one (or more) enablers is gone
                state = MOVE_BACK;  // to 0,0,0
                input.in = 0;

                //if (!is_paused && is_on && move_enable) {
                if (!is_paused && is_on) {
                    // is_paused removed, move back but limit rate
                    data.was_paused = 1;
                    move = 1;
                } else if (!is_on) {
                    // is_on removed, move back, no rate limit
                    move = 1;
                    reset_old(data);
                } else if (!move_enable) {
                    // move_enable removed, move back, no rate limit
                    move = 1;
                } else {
                    //UNEXPECTED
                    rtapi_print_msg(RTAPI_MSG_ERR,
                    "moveoff.comp: Unexpected: "
                    "state=%dis_paused=%d is_on=%d move_enable=%d\n",
                    state,is_paused,is_on,move_enable);
                }
                break;
          case  MOVE_BACK:
                move = 1;
                input.in = 0;

                if (!data.gave_msg && !is_paused && ! offset_removed(data, epsilon)) {
                    // is_paused deactivated
                    data.gave_msg = 1;
                    rtapi_print_msg(RTAPI_MSG_ERR,
                    "moveoff.comp: ERROR:\nProgram resumed before offsets removed\n"
                    "joint %i: %f\n",
                    i, data.old_out[i]);
                }
                data.was_paused = 0;
                if (offset_removed(data, epsilon)) {
                    state = IDLE;
                    move = 0;
                    reset_old(data);
                }
                break;
        }

        if (move) {
            input.minlimit  = offset_min(i);
            input.maxlimit  = offset_max(i);
            input.maxvlimit = offset_vel(i);
            input.maxalimit = offset_accel(i);
            input.old_in    = data.old_in[i];
            input.old_out   = data.old_out[i];
            input.old_v     = data.old_v[i];
            input.lin       = data.old_lin[i];

            lim3(input,&old_in,&lout,&old_v);
            data.old_in[i] = old_in;
            data.old_v[i] = old_v;
            plusoffset(i) = pos(i) + lout;
            offset_current(i) = lout;
            fb_minusoffset(i) = fb(i) - lout;
            data.old_out[i] = lout;
            data.old_v[i]   = old_v;

        } else {
            plusoffset(i) = pos(i);
            offset_current(i) = 0;

            fb_minusoffset(i) = fb(i);

        }
        offset_applied = ! offset_removed(data, epsilon);
    }
}

EXTRA_SETUP(){
    if (personality == 0) personality = 3;
    return 0;
}