#include "KMotionDef.h"

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

#define CANON_UNITS_INCHES 1  

#define ZAXIS 2			// Axis to slave
#define SPINDLE_AXIS 3	// Spindle Axis With Encoder Feedback 
#define Z_CPI 83230.72		// Counts per inch of slave axis
#define CPR 4000.00		// Counts per rev of spindle encoder
#define TAU 0.001		// Smoothing time
#define MAXTAPSPEED 850 // Maximum Tapping speed in rpm
#define INDEX_SPEED 1000

float SlaveGain,ToCut,Z0,S0,OS,SpindleStopTime,TotalCut;
float zMin;

main()
{
	// 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 #define CANON_UNITS_INCHES 1  #define CANON_UNITS_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+0];
	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];

	printf("MCode = %f\n",MCode); 
	printf("Bottom = %f\n",Bottom); 
	printf("Retract = %f\n",Retract); 
	printf("Peck = %f\n",Peck); 
	printf("Rate = %f\n",Pitch); 
	printf("RPM = %f\n",RPM);

	if (Units == CANON_UNITS_INCHES)
		printf("Units = Inches\n"); 
	else
		printf("Units = mm\n"); 

	//printf("Axis = %d\n",Axis); 
	//printf("AxisRes = %f\n",AxisRes); 

	float TPI = 1.0 / Pitch;
	//printf("TPI: %f\n", TPI);
    float FeedRate 	= RPM/(TPI*60);
	//printf("feedrate: %f\n", FeedRate);

    float TotalDepth = (Bottom - Retract); //total depth to cut
	//printf("TD = %f\n",TotalDepth); 
	float ToCut = (TotalDepth * CPR * TPI) *-1;
	//printf("TC = %f\n",ToCut); 

    
    
    void DoSlave(void);
    void DoTap(float Dist, float Rate, float TPI);

    
	//printf("ZPI: %f\n", Z_CPI);
	//printf("CPR: %f\n", CPR);
    SlaveGain = Z_CPI/(CPR * TPI);
		

	
	
    
	//index spindle
	Jog(SPINDLE_AXIS, 0);
	while (!ReadBit(42)) WaitNextTimeSlice();
   
    Jog(SPINDLE_AXIS, INDEX_SPEED);
    while (!ReadBit(42)) WaitNextTimeSlice();
	Zero(SPINDLE_AXIS);

    MoveAtVel(SPINDLE_AXIS, 0, 10);
		while (!CheckDone(SPINDLE_AXIS)) ;
	Delay_sec(1);
	
	Z0 = chan[ZAXIS].Dest;
	S0 = chan[SPINDLE_AXIS].Position;;
	//printf("s0: %f\n", S0);
	//printf("Z0: %f\n", Z0);

	
		
	DoTap(ToCut, FeedRate, TPI);
	Delay_sec(3); //temporary for reading DRO
	DoTap(-ToCut, FeedRate, TPI);
		
	Delay_sec(1.0);
	//printf("moving to zero");
	Move(ZAXIS,Z0);   // move back to where we started 
	while (!CheckDone(ZAXIS)) ;

	*(double *)&persist.UserData[30]=1.0;  // set flag that we are complete
	printf("Tap Complete\n");


	float Zmininches = (-Z0 - zMin) / Z_CPI * -1;	// Calculate Z min in inches
	Zmininches = Zmininches - Retract;
    printf("Tap Complete. Minimium Depth Reached = %f\n", Zmininches);
}

void DoTap(float Dist, float Rate, float TPI)
{
	

	MoveRelAtVel(SPINDLE_AXIS, Dist, Rate*TPI*CPR);
	while(!CheckDone(SPINDLE_AXIS))
		DoSlave();
    
}

void DoSlave(void)
{ 
	float Destination = (chan[SPINDLE_AXIS].Dest-S0) * SlaveGain + -Z0;	// Set new destination
	MoveExp(ZAXIS, Destination * -1, TAU);	// Move to new destination
	WaitNextTimeSlice();	
	
}