//   This is a 'single axis trajectory planner' component for linuxcnc's HAL
//   This is a conversion of linuxcnc's internal jogging planner to comp
//   see motion/simple_tp.c Author: jmkasunich
//   Copyright 2014 Chris Morley <chrisinnanaimo@hotmail.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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

component simple_tp """\
This component is a single axis simple trajectory planner, same as used for jogging in LinuxCNC.
Used by PNCconf to allow testing of acceleration and velocity values for an axis.""";

pin in float target-pos "target position to plan for.";
pin in float maxvel "Maximum velocity";
pin in float maxaccel "Acceleration rate";
pin in bit enable "If disabled, planner sets velocity to zero immediately.";

pin out float current-pos "position commanded at this point in time.";
pin out float current-vel "velocity commanded at this moment in time.";
pin out bit active "if active is true, the planner is requesting movement.";
function update;
license "GPL";
author "Chris S Morley";
;;
#include "rtapi_math.h"
FUNCTION(update) {
    double max_dv, tiny_dp, pos_err, vel_req;

    active = 0;
    /* compute max change in velocity per servo period */
    max_dv = maxaccel * fperiod;
    /* compute a tiny position range, to be treated as zero */
    tiny_dp = max_dv * fperiod * 0.001;
    /* calculate desired velocity */
    if (enable) {
	/* planner enabled, request a velocity that tends to drive
	   pos_err to zero, but allows for stopping without position
	   overshoot */
	pos_err = target_pos - current_pos;
	/* positive and negative errors require some sign flipping to
	   avoid sqrt(negative) */
	if (pos_err > tiny_dp) {
	    vel_req = -max_dv +
		       sqrt(2.0 * maxaccel * pos_err + max_dv * max_dv);
	    /* mark planner as active */
	    active = 1;
	} else if (pos_err < -tiny_dp) {
	    vel_req =  max_dv -
		       sqrt(-2.0 * maxaccel * pos_err + max_dv * max_dv);
	    /* mark planner as active */
	    active = 1;
	} else {
	    /* within 'tiny_dp' of desired pos, no need to move */
	    vel_req = 0.0;
	}
    } else {
	/* planner disabled, request zero velocity */
	vel_req = 0.0;
    }
    /* limit velocity request */
    if (vel_req > maxvel) {
        vel_req = maxvel;
    } else if (vel_req < -maxvel) {
	vel_req = -maxvel;
    }
    /* ramp velocity toward request at accel limit */
    if (vel_req > current_vel + max_dv) {
	current_vel += max_dv;
    } else if (vel_req < current_vel - max_dv) {
	current_vel -= max_dv;
    } else {
	current_vel = vel_req;
    }
    /* check for still moving */
    if (current_vel != 0.0) {
	/* yes, mark planner active */
	active = 1;
    }
    /* integrate velocity to get new position */
    current_pos += current_vel * fperiod;
}