component caxis "Control a spindle as a C-axis";

pin in bit spindle-on;
pin in float spindle-revs;
pin in float spindle-velocity-in;
pin out float spindle-velocity-out;
param rw float spindle-maxaccel = 50;
pin out float position-fb-out;
pin out bit pid-enable;
pin in float pid-in;
pin out bit spindle-at-speed;
pin out signed state;

pin in float switchkinstype = 0;

license "GPL";
author "andypugh";

function _;
;;

#include "rtapi_math.h"

FUNCTION(_){
    static float offset;
    static float prev_switchkinstype = 0;

    switch state{
        case 0: // C-axis mode
            if (spindle_on){
                state = 1;
            }
            pid_enable = 1;
            spindle_velocity_out = pid_in;
            // must be set OK as else when using spindle 1, the spindle at speed check in linuxcnc
            // fails (as it checks ALL spindles)
            spindle_at_speed = 1; 
            
            // on exit from polar, do as on exit from spindle mode. Gets rid of need
            // to unwind. Ref comments in polar_userkins forward kinematics 
            if ((prev_switchkinstype != switchkinstype) && (switchkinstype == 0)) {
              offset = round(spindle_revs);
            }
            else {
                // detect index-reset
                if (fabs(position_fb_out - (spindle_revs - offset) * 360) > 180){
                    offset = round(spindle_revs);
                }
            }
           
            position_fb_out = (spindle_revs - offset) * 360;
            break;
        case 10:
          break;    
        case 1: // Spindle has been enabled, do accel
            pid_enable = 0;
            spindle_at_speed = 0;
            if (! spindle_on){
                state = 3;
                break;
            }
            if (spindle_velocity_in > 0){
                spindle_velocity_out += spindle_maxaccel * fperiod;
                if (spindle_velocity_out >= spindle_velocity_in){
                    state = 2;
                }
            } else if (spindle_velocity_in < 0){
                spindle_velocity_out -= spindle_maxaccel * fperiod;
                if (spindle_velocity_out <= spindle_velocity_in){
                    state = 2;
                }
            } 
            break;
        case 2: // spindle running normally
            if (! spindle_on){
                state = 3;
                spindle_at_speed = 0;
                break;
            }
            spindle_velocity_out = spindle_velocity_in;
            spindle_at_speed = 1;
            break;
        case 3: // spindle ramp to zero
            if (spindle_on){
                state = 1;
                break;
            }
            if (fabs(spindle_velocity_out) < spindle_maxaccel * fperiod){
                spindle_velocity_out = 0;
                spindle_at_speed = 1;
                offset = round(spindle_revs);
                state = 0;
                break;
            }
            if (spindle_velocity_out > 0){
                spindle_velocity_out -= spindle_maxaccel * fperiod;
            } else {
                spindle_velocity_out += spindle_maxaccel * fperiod;
            }
        }
        prev_switchkinstype = switchkinstype;
}