#define PROBE_DEBOUNCE_VAL 20

static void process_inputs(void)
{
    int joint_num;
    double abs_ferror, tmp;
    axis_hal_t *axis_data;
    emcmot_joint_t *joint;
    int must_abort;
    static int probe_counter=0;
    static int previous_probe_value=0;

    /* read probe input */
    if(!probe_counter) {
	// We must debounce the probe, but get a change asap so we can't use 
	// hal debouncers as they delay the signal. So we just ignore changes
	// after the first for a fixed amount of time
	// maybe this should be in hal...
	if(*(emcmot_hal_data->probe_input) != previous_probe_value) {
	    previous_probe_value = emcmotStatus->probeVal = *(emcmot_hal_data->probe_input);
	    probe_counter = PROBE_DEBOUNCE_VAL;
	}
    }
    else probe_counter--;
    /* check if the probe has been tripped */
    if (emcmotStatus->probing){
	if (emcmotStatus->probeTripped ) {
	    tpClear(&emcmotDebug->queue);
	    emcmotStatus->probing=0;
	    /* check if the probe hasn't tripped, but the move finished */
	} else if (GET_MOTION_INPOS_FLAG() && 
		( tpQueueDepth(&emcmotDebug->queue) == 0 ) ) {
	    emcmotStatus->probing=0;
	    /* check if the probe trips */
	} else {
	    /* if the probe tripped (val == 1 ) */
	    if (emcmotStatus->probeVal) {
		emcmotStatus->probeTripped = 1; //we have tripped the probe
		emcmotStatus->probedPos = emcmotStatus->carte_pos_fb; //remember the current position
		if (GET_MOTION_COORD_FLAG()) {
		    tpAbort(&emcmotDebug->queue); //if we are in coordinated move, abort
		    SET_MOTION_ERROR_FLAG(0);     //and clear any error flag which might have been set by the abort
		} else { //free mode, abort any joint
		    for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
			/* point to joint struct */
			joint = &joints[joint_num];
			/* tell joint planner to stop */
			joint->free_tp_enable = 0;
			/* stop homing if in progress */
			if ( joint->home_state != HOME_IDLE ) {
			    joint->home_state = HOME_ABORT;
			}
		    }
		}
	    }
	}
    }
    else {
	// Not probing; take care of probe health...
	/* After having bent twice my probe by doing wrong moves
	 * I decided that a probe contact when moving and not probing is
	 * an emergency stop. But we must take into account that, after having tripped
	 * the probe is still in contact. and must disengage. So we reset the probeTripped
	 * flag (just used for debouncing the probe) as soon as the probe is disengaged
	 * Then, if we move and the probe trips, we stop asap.
	 */
	if (emcmotStatus->probeTripped) {
	    // we just did a probe before, and it was not already disengaged
	    if(! emcmotStatus->probeVal) {
		// and we have just disengaged so probe is not in contact anymore
		// so reset probe flag
		emcmotStatus->probeTripped = 0;
	    }
	    // else wait for probe to disengage
	}
	else {
	    if(emcmotStatus->probeVal && !emcmotStatus->overrideLimits) {
		// probe is in contact
		must_abort=0;
		// if we are moving... this means that we try to bend the probe :-(
		// seing if we move is not trivial...
		if (((GET_MOTION_COORD_FLAG()) || (GET_MOTION_TELEOP_FLAG()))
		    && !emcmotDebug->queue.done) {
		    // coordinated move or teleop, and physically moving
		    must_abort=1;
		} else { //free mode, check if any joint is moving
		    for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
			// point to joint struct
			joint = &joints[joint_num];
			if(joint->free_tp_active){
			    must_abort = 1;
			}
		    }
		}
		if(must_abort){
		    if(emcmotDebug->enabling){
			// and estop
			reportError("Error: Probe tripped during non-probing move.");
			emcmotDebug->enabling = 0;
		    }
		}
	    }
	}
    }
    /* read spindle angle (for threading, etc) */
    ...