#include "KMotionDef.h"
#include "MySpindleDefs.h"

#define TAP_VAR 50 // start of where parameters should be placed


//RIGID TAPPING USING SERVO SPINDLE POSITIONS    VERSION 4-25-25

#define ZAXIS 2			           // Axis to slave
#define SPINDLE_DRIVE_AXIS 3       // Spindle motor axis, this is the commanded axis
#define SPINDLE_ENC_AXIS 4         // Spindle encoder "dummy" axis
#define MAXTAPSPEED 850            // Maximum Tapping speed in rpm
#define TAU 0.001		           // Smoothing time //Tau is time constant for moveexp command
#define ESTOP 63		           // Emergency stop bit number-CLEAR TO ACTIVATE ESTOP
#define ESTOPSTATE 0	           // Emergency stop triggered by ClearBit = 0, SetBit = 1 //Only used if max rpm exceeded Not used, but I might add estop if Z axis error gets too large

double SlaveGain,Z0,SD0,SE0,FinalPos,LastPeck,SDA_VEL,NextPeck,PeckAmt;                //Define variables
int Done,MT,FHT,MTrestore;                                                             //Define variables Done, MT(motiontracker, FHT(feedholdtracker)),MTrestore(put MT back after feedhold) 

void Slave(void);


main()
{
//Variables used in this program
int MCode;
int CPR_SDA = 4096;                 // Counts per MOTOR rev of spindle drive axis ***NOTE: CW ROTATION COUNTS POSITIVE***
int CPR_SEA = 2000;                 // Counts per SPINDLE rev from spindle encoder ***NOTE: CW ROTATION COUNTS NEGATIVE***
int Z_CPI = 50000;		            // Counts per inch of slave (Z) axis ***Positive values are -Z***
float DRIVE_RATIO = 2.0;            // Ratio of spindle revs to motor revs. 2.0 in HIGH range and 0.5 in LOW range

// Set variables 

//Note G84 passes the following parameters: Z(bottom) R(retract) Q(peck) F(pitch)
//Setup M119 to pass to Var 50

	// Call MCode M119 to do Rigid Tapping
	// 
	// Var+0 - (int) 119 (MCode number)
	// Var+1 - (float) Bottom Z in User Units
	// Var+2 - (float) Retract Position in User Units
	// Var+3 - (float) Distance per Peck in User Units (0 if unspecified)
	// Var+4 - (float) Feed Rate F Number (User Units/Rev)
	// Var+5 - (float) Spindle RPM to Tap
	// Var+6 - (int) Units (INCHES=1 MM=2)
	// Var+7 - (int) KFLOP axis to move (derived from active GCode Plane and Coord Motion System defs)
    // Var+8 - (float) Axis Resolution Counts/inch

	int MCode = persist.UserData[TAP_VAR];
	float Bottom = *(float *)&persist.UserData[TAP_VAR+1];
	float Retract = *(float *)&persist.UserData[TAP_VAR+2];
	float Peck = *(float *)&persist.UserData[TAP_VAR+3];
	float Pitch = *(float *)&persist.UserData[TAP_VAR+4];
	float RPM = *(float *)&persist.UserData[TAP_VAR+5];
	int Units = persist.UserData[TAP_VAR+6];
	int Axis = persist.UserData[TAP_VAR+7];
	float AxisRes = *(float *)&persist.UserData[TAP_VAR+8];

	if (Units == 2) // if metric convert units  // NOT TESTED IN mm MODE!!! MAY NOT WORK CORRECTLY!!!
	{
		Bottom = Bottom / 25.4;          // Convert Bottom from mm to inches  
		Retract = Retract / 25.4;        // Convert Retract from mm to inches
		Pitch = Pitch / 25.4;            // Convert Pitch from mm to inches
		Peck = Peck / 25.4;              // Convert Peck from mm to inches
	}


    if (persist.UserData[21]==1)      //If spindle is in low range
    DRIVE_RATIO=0.5;                //Set low range drive ratio


    float Depth = Bottom - Retract;	// Depth equal to bottom and retract height                     ***This will be a negative number***
	if (Peck == 0) Peck = Depth * -1.1;	// if peck = zero, set larger then depth so its ignored	    ***This results in positive value***
	

// Check if Max Speed exceeded

	if (RPM > MAXTAPSPEED) 
	{
		SetStateBit(ESTOP,ESTOPSTATE);	// e-stop
		printf("Max Tap Speed Exceeded\n");		// Print error
		return 0;  					// End
	}

//If spindle is rotating, stop
Jog(3,0);                                                       // Command spindle stop
while(!(CheckDone(3)));                                         // Loop until spindle is stopped
persist.UserData[STATEVAR] = 0;                                 // Remember spindle is Off


//Prep tapping parameters 

    SlaveGain = Z_CPI * Pitch  / CPR_SEA;		                // Set Slave factor using spindle encoder axis    
    Z0 = chan[ZAXIS].Position;					                // Record Z start position                          //Units Z axis counts
    SD0=chan[SPINDLE_DRIVE_AXIS].Dest;                          // Record spindle drive axis start position         //Units Spindle Drive Axis counts
    SE0=chan[SPINDLE_ENC_AXIS].Position;                        // Record spindle encoder axis start position       //Units Spindle Encoder counts
    FinalPos=SD0+(-1*CPR_SDA*Depth)/(DRIVE_RATIO*Pitch);        // Calculate spindle drive axis position at bottom  //Units Spindle Drive Axis counts ***CW rotation moves axis POSITIVE***
    PeckAmt=(CPR_SDA*Peck)/(DRIVE_RATIO*Pitch);                 // Calculate spindle drive axis position change per peck //Units Spindle Drive Axis counts ***Positive number***
    LastPeck=SD0;                                               // Set last peck position at starting position      //Units Spindle Drive Axis counts
    NextPeck=SD0;                                               // Set next peck position at starting position      //Units Spindle Drive Axis counts
    SDA_VEL=(CPR_SDA*RPM)/(DRIVE_RATIO*60);                     // Spindle drive velocity                           //Units Spindle Drive Axis Counts/Sec
    Done=0;                                                     // Set tap cycle state
    MT=0;                                                       // Set motion tracker to 0

//Begin tapping//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//////Begin while loop/////////////////////////////

while (Done == 0)                                               // Loop continues until Done is set to 1
{

////////////////////////Feedhold disable section ////////////////////////////////////////
	if ((CS0_StoppingState != 0))                  // If feedhold is on
	{
		ResumeCoordinatedMotion();                                // Release feedhold	
	}


    Slave();                                                    // Slave Z axis to spindle encoder axis 
    /////////////////////Set NextPeck for first loop//////////////////////////////////////////////////////////////////////////////////
    if (((Peck + Depth) >= 0) || (Peck == 0))                   // If next peck amount would equal or exceed final depth (Peck is positive or zero, Depth is negative)
    {
        NextPeck = FinalPos;                                    // Set next peck to tap to final depth
    }
    else if (((Peck + Depth) < 0) && (MT == 0))                 // If peck smaller than total depth AND this is the first loop
    {
        NextPeck = SD0+PeckAmt;                                 // Set NextPeck to 1 peck depth below start
    }
    /////////////////////Reset for new loop section////////////////////////////////////////////////////////////////////////////////
    else if (MT == 5)                                           // If last peck cycle complete and loop did not exit (Done still == 0)
    {
        MT=0;                                                   // Reset motion tracker for new loop
    }
    /////////////////////Start Downward Motion///////////////////////////////////////////////////////////////////////////////////
    if (MT == 0)                                                // If motion tracker is 0 (downward motion not yet begun)
    {
        printf("NextPeck = %f\n",NextPeck);
        MoveAtVel(SPINDLE_DRIVE_AXIS,NextPeck,SDA_VEL);         // Move spindle axis to NextPeck position                  
        MT=1;                                                   // Set motion tracker to 1 (downward motion in progress)
    }
    else if (MT == 1)                                           // If motion tracker indicates downward motion in progress
    {
        if (CheckDone(SPINDLE_DRIVE_AXIS))                      // If motion is complete
        {
            MT=2;                                               // Set motion tracker to 2 (Motion complete at bottom)
        }
    }
    //////////////////Start Upward Motion///////////////////////////////////////////////////////////////////////////////////////////
    else if (MT == 2)                                           // If motion tracker indicates motion complete at bottom
    {
        MoveAtVel(SPINDLE_DRIVE_AXIS,SD0, SDA_VEL);             // Move spindle axis back to retract position
        MT=3;                                                   // Set motion tracker to 3 (upward motion in progress)
    }
    else if (MT == 3)                                           // If motion tracker indicates upward motion in progress
    {
        if (CheckDone(SPINDLE_DRIVE_AXIS))                      // If motion is complete
        {
            MT=4;                                               // Set motion tracker to 4 (upward motion complete)
            LastPeck = NextPeck;                                // Set LastPeck to the NextPeck depth just completed
        }
    }
    /////////////////Prep for next action/////////////////////////////////////////////////////////////////////////////////////////
    else if (MT == 4)                                           // If motion is complete at retract postion
    {
        if (LastPeck == FinalPos)
        {
            Done=1;                                             // Set Done to 1 to exit while loop, tapping cycle complete
        }
        else if ((LastPeck+PeckAmt) >= FinalPos)                // If next peck would equal or exceed final depth
        {
            NextPeck = FinalPos;                                // Set NextPeck to final depth
            MT = 5;                                             // Set motion tracker to 5 (ready for next cycle)
        }
        else                                                    // Next peck will not equal or exceed final depth
        {
            NextPeck = LastPeck + PeckAmt;                      // Set NextPeck 
            MT = 5;                                             // Set motion tracker to 5 (ready for next cycle)
        }
    }

}   //End while loop

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//                              Tapping Complete
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	MoveAtVel(ZAXIS,Z0,10000); //Small move, needed to return Z axis exactly to retract plane to end cycle
}	//End main function


void Slave(void)
{
	float Destination = (chan[SPINDLE_ENC_AXIS].Position-SE0) * SlaveGain + Z0;	// Set new destination  //Units Z axis counts
	MoveExp(ZAXIS, Destination, TAU);	// Move to new destination                                      
	WaitNextTimeSlice();	
}