#include "KMotionDef.h"
#define TMP 10					// which spare persist to use to transfer data
#include "..\..\KflopToKMotionCNCFunctions.c"

//-----------------------------------------
//      ROTARY TOOL CHANGE WITH STEPPER MOTOR CAROUSEL
//-----------------------------------------
// CREATED BY: Dynomotion
// MODIFIED BY: Noel Nogal 
// NAME: STEPPER MOTOR ROTARY TOOL CHANGER PROGRAM CAROUSEL ONLY ON X AXIS
// DATE: 02/ENE/24
//-----------------------------------------


#define AXISX 0					//ACTIVATE AXIS X
#define AXISY 1					//ACTIVATE AXIS Y
#define AXISZ 2					//ACTIVATE AXIS Z
#define AXISA 3					//ACTIVATE AXIS A TO DRIVE A CAROUSEL STEPPER MOTOR

//Absolute position in degrees of tool holders

#define PLATE_A_1 1				//ROTATING POSITION ON A AXIS CAROUSEL TO GRAB TOOL 1
#define PLATE_A_2 2				//ROTATING POSITION ON A AXIS CAROUSEL TO GRAB TOOL 2
#define PLATE_A_3 3				//ROTATING POSITION ON A AXIS CAROUSEL TO GRAB TOOL 3
#define PLATE_A_4 4				//ROTATING POSITION ON A AXIS CAROUSEL TO GRAB TOOL 4
#define PLATE_A_5 5				//ROTATING POSITION ON A AXIS CAROUSEL TO GRAB TOOL 5
#define PLATE_A_6 6				//ROTATING POSITION ON A AXIS CAROUSEL TO GRAB TOOL 6
#define PLATE_A_7 7				//ROTATING POSITION ON A AXIS CAROUSEL TO GRAB TOOL 7
#define PLATE_A_8 8				//ROTATING POSITION ON A AXIS CAROUSEL TO GRAB TOOL 8


#define HOLDER_X -38			//POSICION EXACTA DEL TOOLHOLDER EN CAROUSEL EN X (FUNCION 3)
#define HOLDER_Y 0				//POSICION EXACTA DEL TOOLHOLDER EN CAROUSEL EN Y (FUNCION 3)
#define HOLDER_Z 60.0			//POSICION EXACTA DEL TOOLHOLDER EN CAROUSEL EN Z (FUNCION 2)

//absolute position of the tool height setting plate

#define TOOL_HEIGHT_PLATE_X 0
#define TOOL_HEIGHT_PLATE_Y 0

// absolute position to move to that is permanently unobstructed, and safe to move down in Z

#define TOOL_CHANGE_SAFE_POS_X -35	//POSICION CERCA DEL CARROUSEL EN X LISTO PARA ENGANCHAR (FUNCION 1)
#define TOOL_CHANGE_SAFE_NEG_Y 0	//POSICION CERCA DEL CARROUSEL EN Y LISTO PARA ENGANCHAR



#define AXIS_SAFE_DISTANCE_X -35	//POSICION DE RETRACTO PARA SACAR EL TOOLHOLDER DEL CARROUSEL
#define AXIS_SAFE_DISTANCE_Y  0

// Spindle IO bits

#define CLAW_EJECT      53			// IO OUTPUT BIT TO CLAMP/ UNCLAMP
#define SPINDLE_CLEAN   54		    // IO OUTPUT BIT TO BLOW AIR INTO THE SPINDLE TO CLEAN DUST
#define CLAW_LOOSE      1165		// IO INPUT BIT TO SENSE WHETHER THE CLAW HAS EJECTED,(CLAW MEANS THE CLAMP OF SPINDLE)
#define TOOL_SENSE      1166		// IO INPUT BIT TO SENSE WHETHER A TOOL IS IN THE SPINDLE
#define EMPTY_HOLDER    1167		// IO INPUT BIT TO SENSE WHETHER A TOOL HOLDER IS EMPTY IN THE CAROUSEL READY TO RECIVE THE TOOL
#define SPNDLE_STATUS   1095	    //(condition)monitor if spindle still spining to avoid tool unclamp
#define DUSTBOOT        1088	    //(condition)monitor if dust boot is in the right position before tool change
#define ORIENTSPDL_DN   1090		//(condition)monitor if spindle is on on down position
#define CARROUSEL_OUT   1033		//(condition)monitor if carrousel is in the right position for tool change

#define TOOL_VAR 9				// Tool changer desired new tool Var

// Tool changer Last tool loaded is saved globally in this Var

#define LAST_TOOL_VAR 8			//  -1=Spindle empty, 0=unknown, 1-4 Tool Slot loaded into Spindle
#define TOOL_DISK_FILE "c:\\Temp\\ToolChangerData.txt"	//CREATE A TEMP FOLDER IN C: FOR RECORD THE LAST TOOL LOADED IN SPINDLE


#define CLAMP_TIME 3.0			//TIME TO WAIT FOR CLAMP/ UNCLAMP
#define TOOL_HEIGHT_BIT	1055	//bit to read tool height plate (KONNECT INPUT 31)

#define SAFE_HEIGHT_Z  1200		//DISTANCE TO CLEAR THE SPINDLE UP AWAY FROM HOLDER BEFORE ROTATE TO NEW TOOL

#define TOOL_RETRACT_SPEED_Z 1400	//SPEED TO MOVE Z AXIS UP AFTER HOLDER HAS BEEN EJECTED

#define PLATE_ROTATE_SPEED_A 6	//A AXIS SPEED TO ROTATE CAROUSEL

#define RapidMove 340.0			//SPEED ON RAPID MOVE TO POSITION NEAR CAROUSEL
#define SlowSpeed 60.0			//SPEED ON X,Y,Z AXIS TO CHANGE TOOL
#define EngageSpeed 320.0		//SPEED ON Z AXIS WHEN GO DOWN TO ENGAGE A TOOL

#define CNT_PER_IN_X 14600.0		//MOVE DISTANCE X AXIS NEAR TO CAROUSEL
#define CNT_PER_IN_Y 1200.0		//SPEED AND DISTANCE Y AXIS
#define CNT_PER_IN_Z 140.0		//SPEED AND DISTANCE Z AXIS
#define CNT_PER_DEGREE_A 600



// function prototypes
//int vlast = 0, vlastsolid = -1, vcount = 0;	//spindle status, spinning or stop

int DoToolChange(int ToolSlot);
int GetCurrentTool(int *tool);
int SaveCurrentTool(int tool);
BOOL ToolNumberValid(int tool);
float ToolPositionA(int tool);
float ToolPositionX(int tool);
float ToolPositionY(int tool);
int MoveXY(float y, float x, float Speed);
int MoveZ(float z, float Speed);
int MoveA(float a, float Speed);
int UnloadTool(int CurrentTool);
int LoadNewTool(int CurrentTool, int Tool);
int EjectTool(void);


int main()
{
	int ToolSlot = persist.UserData[TOOL_VAR];	// Requested tool to load (value stored an integer) 

	if (DoToolChange(ToolSlot))	// perform Tool Change
	{
		// error, Halt Job
		DoPC(PC_COMM_HALT);
	}
}


// Perform Tool Change.  Return 0=Success, 1=Failure
int DoToolChange(int ToolSlot)
{
	int CurrentTool;

	if (GetCurrentTool(&CurrentTool))
		return 1;				//  -1=Spindle empty, 0=unknown, 1-8 Tool Slot loaded into Spindle

	printf("Load Tool Slot %d requested, Current Tool %d\n", ToolSlot, CurrentTool);

	if (!ToolNumberValid(ToolSlot))	// check if invalid
	{
		char s[80];
		sprintf(s, "Invalid Tool Change Number %d\n", ToolSlot);
		printf(s);
		MsgBox(s, MB_ICONHAND | MB_OK);
		return 1;
	}

	if (CurrentTool != ToolSlot)	// need to load different tool?


		// unload requested tool
		if (CurrentTool != -1)	// is there a tool in the Spindle??
			if (UnloadTool(CurrentTool))
				return 1;		// yes, unload it

	// Now Spindle is empty, load requested tool
	if (LoadNewTool(CurrentTool, ToolSlot))
		return 1;

	SaveCurrentTool(ToolSlot);	// save the one that has been loaded
	return 0;					// success
}


//LOAD NEW TOOL (Spindle must be empty)
int LoadNewTool(int CurrentTool, int Tool)
{

	if (CurrentTool != Tool)	// need to load different tool? 
	{
		Delay_sec(1);			// wait few seconds before axis A start rotating

//ROTATE CAROUSEL TO POSITION THE REQUESTED TOOL

		if (MoveA(ToolPositionA(Tool), PLATE_ROTATE_SPEED_A))	
		{

		}

		Delay_sec(1.5);			// wait few seconds before Z axis go down to clamp new tool
       
		// - Move to position of requested tool
		// - Rapid move to absolute position of new tool only in X and Y
		//if (MoveXY(ToolPositionX(Tool), HOLDER_Y, SlowSpeed))
			//return 1;

//DRIVE THE SPINDLE DOWN TO ENGAGE THE TOOLHOLDER

		if (MoveZ(HOLDER_Z, EngageSpeed))	

		return 1;

        printf("FUNCT 37= Move Z down to grab requested tool\n");

		// - Engage new tool
		// - CLAW_EJECT and SPINDLE_CLEAN bits are currently high from tool removal operation
		// - Turn off CLAW_EJECT and SPINDLE_CLEAN bits to engage tool
		ClearBit(CLAW_EJECT);
		ClearBit(SPINDLE_CLEAN);

		// - Wait for time in seconds defined by CLAMP_TIME
		Delay_sec(CLAMP_TIME);

        printf("FUNCT 38= Spindle clamp tool holder\n");

		// - Check to see if CLAW_LOOSE and TOOL_SENSE are high; if either are not, 
		// -something has gone wrong; halt everything and display message indicating failure
		// - Tool has been engaged
		///if (!ReadBit(CLAW_LOOSE))
		///{
		///printf("TOOL NOT ENGAGED\n"); //CONDITION IF TOOL NOT ENGAGED, MACHINE WILL STOP AND SHOW BOX MESSAGE
		///MsgBox("WARNING: TOOL NOT ENGAGED\n", MB_ICONHAND | MB_OK);
		///return 1;
		///}
		///if (!ReadBit(TOOL_SENSE))
		///{
		///printf("Tool Sense Error\n"); //CONDITION IF NO TOOL, MACHINE WILL STOP AND SHOW BOX MESSAGE
		///MsgBox("TOOL SENSE ERROR\n", MB_ICONHAND | MB_OK);
		///return 1;
		///}

//RETRACT TOOLHOLDER BACK ON X AXIS TO CLEAR POSITION

	if (MoveXY(AXIS_SAFE_DISTANCE_X, AXIS_SAFE_DISTANCE_Y, SlowSpeed))

		return 1;

        printf("FUNCT 35= Pull toolholder out of fork\n");


// - RAISE THE TOOL UP TO A SAFE POSITION BEFORE START CUTTING

		if (MoveZ(1200.00, RapidMove))	

        return 1;			
       
        printf("FUNCT 36= Rise tool before spindle start\nDONE    = TOOL CHANGE DONE\n");

        printf("FUNCT 39-0 = Close carousel cover\n");

		ClearBit(55);			//close carousel cover after tool changed

  	}
	    return 0;					//success                  
}


// - Remove tool in spindle by going to holder of current tool
int UnloadTool(int CurrentTool)
{

	ClearBit(48);				//turn spindle off before go get new tool
	ClearBit(49);				//turn orient spindle off before go get new tool
	ClearBit(50);				//turn dust boot off before go get new tool
	Delay_sec(2);				//wait few seconds to complete spindle stop


// - Rapid to TOOL_CHANGE_SAFE_POS to execute a safe negative Z move   (SE POCISIONA CERCA DEL CARRUSEL PARA ACOMODAR EL TOOLHOLDER)
	if (MoveXY(TOOL_CHANGE_SAFE_POS_X, TOOL_CHANGE_SAFE_NEG_Y, SlowSpeed))

		return 1;

        printf("FUNCT   = PERFORM TOOL CHANGE\nFUNCT 34= Move X axis near carousel\n");

// - POSITION Z AXIS TO ALIGN TOOL HOLDER WITH CARROUSEL FORK (SE ALINEA EL Z CON EL CARRUSEL PARA ACOMODAR EL TOOLHOLDER)
	if (MoveZ(380.00, RapidMove))

		return 1;				//MOVE Z TO ALIGN HOLDER TO FORK WHEN START TOOL CHANGE

        printf("FUNCT 33= Align toolholder to fork\n");

//USE SAME VALUE AS #define HOLDER_Z TO KEEP THE SAME HEIGHT
//THIS VALUE AFECTS DISTANCE TO CLEAR THE SPINDLE FROM HOLDER TO MOVE TO NEXTONE


	SetBit(55);					//open carousel cover to grab new tool

    printf("FUNCT 39-1 = Open carousel cover\n");

// - Approach tool holder by matching Z height of tool flange currently in spindle with tool holder claw
//if (MoveZ(HOLDER_Z, SlowSpeed))
//return 1;


	Delay_sec(1);				//wait few seconds to set tool in carousel

// - After matching height above, approach tool holder by moving to holder X position   
	if (MoveXY(ToolPositionX(CurrentTool), TOOL_CHANGE_SAFE_NEG_Y, SlowSpeed))	//PONE EL TOOLHOLDER EN EL CARRUSEL

        return 1;

        printf("FUNCT 32= Set tool holder in fork\n");

// - Read EMPTY HOLDER bit to see whether the tool holder is empty, to make a safe move without  
//   crashing other tool
//   If EMPTY HOLDER bit is high, something has gone wrong;
//   halt everything and display message indicating failure
		if (ReadBit(EMPTY_HOLDER))
		{
			printf("other tool in the way\n");	//CONDITION IF TOOL NOT ENGAGED, MACHINE WILL STOP AND SHOW BOX MESSAGE
			MsgBox("WARNING:\n OTHER TOOL IS IN THE WAY\n CAN'T CONTINUE", MB_ICONHAND | MB_OK);
			SetBit(56);
			return 1;
		}

// - After matching X position, match tool Y position
//   if (MoveXY(ToolPositionX(CurrentTool), HOLDER_Y, SlowSpeed)) //PONE EL TOOLHOLDER EN EL CARRUSEL
//return 1;

// - Move only in Y position until current position matches tool holder position (maybe disable X)                          axis?)
// ???

// - Eject tool

	if (EjectTool())
		return 1;

	return 0;		//success
}


// - Eject tool
int EjectTool(void)
{
	// - Turn on CLAW_EJECT bit to remove tool from spindle
	SetBit(CLAW_EJECT);

    printf("FUNCT 31= Spindle Clamp Ejected\n");

	// - Turn on SPINDLE_CLEAN bit to remove any debris from taper and tools
	SetBit(SPINDLE_CLEAN);

	// - Wait for time in seconds defined by CLAMP_TIME
	Delay_sec(CLAMP_TIME);

	// - Read CLAW_LOOSE bit to see whether the tool is loose, to make a safe Z move without  
	//      destroying tool holder
	// - If CLAW_LOOSE bit is high, something has gone wrong;
	//      halt everything and display message indicating failure
	///if (ReadBit(CLAW_LOOSE))
	///{
	///printf("Claw Loose Error\n");    //CONDITION IF TOOL NOT ENGAGED, MACHINE WILL STOP AND SHOW BOX MESSAGE
	///MsgBox("WARNING: TOOL NOT ENGAGED\n", MB_ICONHAND | MB_OK);
	///return 1;
	///}

// - Move Z axis up at speed defined by 'Z_TOOL_RETRACT_SPEED', to Z_SAFE_HEIGHT (SUBE EL SPINDLE PARA GIRAR EL CARRUSEL)
	if (MoveZ(HOLDER_Z + SAFE_HEIGHT_Z, TOOL_RETRACT_SPEED_Z))

		return 1;
		
		printf("FUNCT 30= move Z up to clear position\n");

//- Read TOOL_SENSE bit to see whether the tool has been successfully ejected from the spindle
// - If TOOL_SENSE bit is high, something has gone wrong; 
//      halt everything and display message indicating failure
	if (ReadBit(TOOL_SENSE))
	{
	printf("Tool Sense Release Error\n");   //CONDITION IF NO TOOL, MACHINE WILL STOP AND SHOW BOX MESSAGE
	MsgBox("TOOL SENSE ERROR\n", MB_ICONHAND | MB_OK);
	return 1;
	}
	return 0; // success

}

//-----------------------------------------------------------------------------
//return a position of tool holder as a function of the tool
float ToolPositionA(int Tool)
{
	return (PLATE_A_2 - PLATE_A_1) * (Tool - 1) + PLATE_A_1;	//GIRA EL CARRUSEL

    
}

//-----------------------------------------------------------------------------
//return x position of tool holder as a function of the tool
float ToolPositionX(int tool)
{
	return (HOLDER_X - HOLDER_X) * (tool - 1) + HOLDER_X;
}

//-----------------------------------------------------------------------------
// Get the last loaded tool.  Parameter points to where to return tool
// First try to get from KFLOP memory
// if memory is invalid, try to read from disk
// if can't read disk then ask Operator
// returns 0 on success, 1 on fail or Operator asked to abort
int GetCurrentTool(int *ptool)
{
	int success, Answer, result, tool;
	float value;

	tool = persist.UserData[LAST_TOOL_VAR];
	success = ToolNumberValid(tool);	// check if valid

	if (!success)				// invalid after power up, try to read from PC Disk File
	{
// Try to open file
		FILE *f = fopen(TOOL_DISK_FILE, "rt");
		if (f)					// did file open?
		{
			// read a line and convert it
			result = fscanf(f, "%d", &tool);
			fclose(f);

			if (result == 1 && ToolNumberValid(tool))
			{
				printf("Read Disk File Value of %d\n", tool);
				success = TRUE;	// success if one value converted
			}
		}

		if (!success)
			printf("Unable to open/read file:%s\n", TOOL_DISK_FILE);
	}

	if (!success)				// if still no success ask Operator
	{
		Answer = InputBox("Tool in Spindle or -1", &value);
		if (Answer)
		{
			printf("Operator Canceled\n");
			return 1;
		}
		else
		{
			tool = value;
			printf("Operator Entered Value of %d\n", tool);
		}
	}

	if (!ToolNumberValid(tool))	// check if invalid
	{
		char s[80];
		sprintf(s, "Invalid Current Tool Number %d\n", tool);
		printf(s);
		MsgBox(s, MB_ICONHAND | MB_OK);
		return 1;
	}

	printf("Current tool = %d\n", tool);
	*ptool = tool;				// return result to caller
	return 0;					//success
}

//-----------------------------------------------------------------------------
// save the tool number to KFLOP global Variable and to PC Disk file in case we loose power
int SaveCurrentTool(int tool)
{
	persist.UserData[LAST_TOOL_VAR] = tool;
	FILE *f = fopen(TOOL_DISK_FILE, "wt");
	fprintf(f, "%d\n", tool);
	fclose(f);
	return 0;
}

//-----------------------------------------------------------------------------
// check if Current Tool number Valid
// -1 = no tool loaded
// 1-8 = valid tool
BOOL ToolNumberValid(int tool)
{
	return tool == 1 - 8 || (tool >= 1 && tool <= 8);
}

//-----------------------------------------------------------------------------
// Move Axis XY at specified Speed and wait until complete
// return 0 = success, 1 if axis disabled
int MoveXY(float x, float y, float Speed)
{
	MoveAtVel(AXISX, x * CNT_PER_IN_X, Speed * CNT_PER_IN_X);
	//MoveAtVel(AXISY, y * CNT_PER_IN_Y, Speed * CNT_PER_IN_Y); (USE THIS CODE ONLY IF Y AXIS IS USED FOR ATC)

	while (!CheckDone(AXISX) || !CheckDone(AXISY))
	{
		if (!chan[AXISX].Enable)
		{
			printf("Error X Axis Disabled\n");
			MsgBox("Error X Axis Disabled\n", MB_ICONHAND | MB_OK);
			return 1;
		}
		//if (!chan[AXISY].Enable)    (USE THIS CODE ONLY IF Y AXIS IS USED FOR ATC)
		//{
			//printf("Error Y Axis Disabled\n");
			//MsgBox("Error Y Axis Disabled\n", MB_ICONHAND | MB_OK);
			//return 1;
		//}
	}
	return 0;					//success
}

//-----------------------------------------------------------------------------
// Move Axis Z at specified Speed and wait until complete
// return 0 = success, 1 if axis disabled
int MoveZ(float z, float Speed)
{
	MoveAtVel(AXISZ, z * CNT_PER_IN_Z, Speed * CNT_PER_IN_Z);

	while (!CheckDone(AXISZ))
	{
		if (!chan[AXISZ].Enable)
		{
			printf("Error Z Axis Disabled\n");
			MsgBox("Error Z Axis Disabled\n", MB_ICONHAND | MB_OK);
			return 1;
		}
	}
	return 0;					//success
}

//-----------------------------------------------------------------------------
// Move Axis A at specified Speed and wait until complete
// return 0 = success, 1 if axis disabled
int MoveA(float a, float Speed)
{
	MoveAtVel(AXISA, a * CNT_PER_DEGREE_A, Speed * CNT_PER_DEGREE_A);

    printf("FUNCT 40= Carousel rotates to requested tool\n");

	while (!CheckDone(AXISA))
	{
		if (!chan[AXISA].Enable)
		{
			printf("Error A Axis Disabled\n");
			MsgBox("Error A Axis Disabled\n", MB_ICONHAND | MB_OK);

			return 1;

		}

	}
	return 0;					//success

}