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

// CREATED BY: Dynomotion
// MODIFIED BY: Noel Nogal 
// NAME: GENEVA TYPE ROTARY TOOL CHANGER PROGRAM FOR WEEKY VENTURE CNC ROUTER WITH ATC.
// DATE: 04/11/23 Chihuahua, Chih. Mexico
// MACHINE OWNER; Julian Villagran

#define AXISX 0					//ACTIVATE AXIS X
#define AXISY 1					//ACTIVATE AXIS Y
#define AXISZ 2					//ACTIVATE AXIS Z
#define MotorON 59				//ACTIVATE CAROUSEL MOTOR FOR PERFORM TOOL CHANGE

///////   THIS ARE THE INPUT BITS READING FOUR BINARY SENSORS

#define BITA 1040				//SENSOR A    // D   A   B   C
#define BITB 1041				//SENSOR B
#define BITC 1042				//SENSOR C    // 0   0   0   1    TOOL 1
#define BITD 1089				//SENSOR D    // 0   0   1   0    TOOL 2
											  // 0   0   1   1    TOOL 3
											  // 0   1   0   0    TOOL 4
											  // 0   1   0   1    TOOL 5
											  // 0   1   1   0    TOOL 6
											  // 0   1   1   1    TOOL 7 
											  // 1   0   0   0    TOOL 8
//-------------------

#define HOLDER_X 0				//POSITION FOR HOLDER ON X AXIS
#define HOLDER_Y 10.4605		//POSITION ON Y AXIS FOR ALL HOLDERS IN CAROUSEL TO CLAMP AND UNCLAMP (15.1180 in positive from 0)
#define HOLDER_Z -992			//DISTANCE THAT ALIGN THE TOOL HOLDER IN SPINDLE WITH CAROUSEL FORK

//////-------------------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 0	//MOVE THE X AXIS NEAR MAGAZINE AWAY FROM FORKS
#define TOOL_CHANGE_SAFE_NEG_Y -27.9430	//MOVE Y AXIS POSITIVE NEAR TO MAGAZINE POSITION AWAY FROM FORKS (11 in positive from 0)

//////-----------------------------

#define AXIS_SAFE_DISTANCE_Y -35.0	// DISTANCE ON Y AXIS TO PULL OUT THE NEW TOOL HOLDER AWAY FROM MAGAZINE

//----------------------------- TOOL CHANGE CONDITIONS

#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 DEBRIS
#define CLAW_LOOSE 1165			// IO INPUT BIT TO SENSE WHETHER THE CLAW HAS UNCLAMPED,(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 IF THE TOOL HOLDER IS EMPTY IN THE CAROUSEL READY TO RECIVE THE TOOL
#define CAROUSEL_COVER 1164		// IO INPUT BIT TO SENSE IF THE CAROUSEL COVER REMAINS OPEN WHILE PERFORM TOOL CHANGE OPERATION
#define HOLDER_POSITION 1162	// IO INPUT BIT TO SENSE IF TOOL HOLDER IS IN THE RIGHT POSITION TO RECIVE THE TOOL,(AVOID CRASH)

#define ESTOP 1172				//(condition)stops the machine when emergency switch is activated

//-----------------------------

#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"


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

#define SAFE_HEIGHT_Z 250.0		//DISTANCE TO CLEAR THE SPINDLE FROM HOLDER TO MOVE TO NEXTONE

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

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

#define RapidMove 180.0			//SPEED ON RAPID MOVE TO POSITION NEAR CAROUSEL
#define SlowSpeed 3.5			//SPEED ON X,Y,Z AXIS TO CHANGE TOOL
#define EngageSpeed 90.0		//SPEED ON Z AXIS WHEN GO DOWN TO ENGAGE A TOOL
#define PulloutSpeed 6.0		//SPEED ON Y AXIS WHEN PULL THE TOOLHOLDER OUT FROM FORK

#define CNT_PER_IN_X 1200.0		//SPEED AND DISTANCE X AXIS
#define CNT_PER_IN_Y 10000.0	//SPEED AND DISTANCE Y AXIS
#define CNT_PER_IN_Z 420.0		//SPEED AND DISTANCE Z AXIS


// function prototypes for compiler
int DoPC(int cmd);
int DoPCFloat(int cmd, float f);
int Debounce(int n, int *cnt, int *last, int *lastsolid);

// function prototypes
int DoToolChange(int ToolSlot);
int GetCurrentTool(int *tool);
int SaveCurrentTool(int tool);
BOOL ToolNumberValid(int tool);
float ToolPositionX(int tool);
int CarouselPosition(void);
int CarouselPositionStable(void);
int MoveXY(float x, float y, float Speed);
int MoveZ(float z, float Speed);
int UnloadTool(int CurrentTool);
int LoadNewTool(int CurrentTool, int Tool);
int EjectTool(void);


int DoPC(int cmd);
int DoPCInt(int cmd, int i);
#define GATH_OFF 0				// define the offset into the Gather buffer where strings are passed

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

		while (SetBit(MotorON), CarouselPosition() != Tool)	//turn carousel motor on
		{

		}

		ClearBit(MotorON);		//turn carousel motor off

		Delay_sec(1);			// 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;


		//- if carousel cover is not in open position operation will halt to avoid possible crash
		//while (!ReadBit(CAROUSEL_COVER))

		//{
		///printf("carousel is not in open position\n"); 
		//MsgBox("WARNING:\n CAROUSEL COVER IS NOT IN OPEN POSITION\n CAN'T CONTINUE", MB_ICONHAND | MB_OK);
		//SetBit(56);
		//return 1;
		//}

		//-if tool holder is not in the right position, operation will halt to avoid possible crash
		//while (!ReadBit(HOLDER_POSITION))

		//{
		//printf("tool holder is not in the right position\n");
		//MsgBox("CRASH WARNING:\n TOOL HOLDER IS OUT OF POSITION\n CAN'T CONTINUE", MB_ICONHAND | MB_OK);
		//SetBit(56);
		//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");

		// - Clamp 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"); 
		///MsgBox("WARNING: TOOL NOT ENGAGED\n", MB_ICONHAND | MB_OK);
		///return 1;
		///}

		//- if there is no tool in the spindle, will halt operation
		//if (!ReadBit(TOOL_SENSE))

		///{
		///printf("Tool Sense Error\n"); 
		///MsgBox("TOOL SENSE ERROR\n", MB_ICONHAND | MB_OK);
		///return 1;
		///}

		//-if carousel cover doesn't remains open while tool change operation will halt to avoid crash
		//while (!ReadBit(CAROUSEL_COVER)) 
		//{
		//printf("carousel cover is not in open position\n");   
		//MsgBox("WARNING:\n CAROUSEL COVER IS NOT IN OPEN POSITION\n CAN'T CONTINUE",MB_ICONHAND | MB_OK);
		//SetBit(56);
		//return 1;
		//}

//RETRACT TOOLHOLDER OUT OF THE FORK ON Y AXIS TO CLEAR POSITION

		if (MoveXY(ToolPositionX(Tool), HOLDER_Y + AXIS_SAFE_DISTANCE_Y, PulloutSpeed))	//SACA EL TOOLHOLDER DEL CARRUSEL
			return 1;

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

		// - Rapid to Z home
		//if (MoveZ(186.20, SlowSpeed)) // PONE EL EJE Z EN ESA POCISION
		//return 1;         //MOVE Z AXIS TO A SPECIFIC DISTANCE WHEN PULL A TOOL OUT 
		//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
		//ClearBit(55);         //close carousel cover after tool changed

	}
	return 0;					//success                  
}


//END


//START
//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 shoe off before go get new tool
	SetBit(60);					//pull dust boot up
	Delay_sec(2);				//wait few seconds to complete spindle stop
	SetBit(55);					//open carousel cover to grab new tool



//RAPID TO TOOL_CHANGE_SAFE_POS TO EXECUTE A SAFE NEGATIVE Z MOVE    

	if (MoveXY(TOOL_CHANGE_SAFE_POS_X, TOOL_CHANGE_SAFE_NEG_Y, RapidMove))
		return 1;

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


// ROTATE CAROUSEL IF SLOT MISMATCH CURRENT TOOLHOLDER NUMBER IN THE SPINDLE BEFORE EXECUTE UNLOAD ACTION,(AVOID CRASHING THE TOOLS).


	while (SetBit(MotorON), CarouselPosition() != CurrentTool)	// need to load different tool?
	{

	}
	ClearBit(MotorON);			//turn carousel motor off

//APPROACH TOOL HOLDER BY MATCHING Z HEIGHT OF TOOL FLANGE CURRENTLY IN SPINDLE WITH FORK
//THIS VALUE POSITION Z AXIS TO ALIGN TOOL HOLDER WITH CARROUSEL FORK.
//USE THE SAME VALUE AS #define HOLDER_Z TO KEEP THE SAME HEIGHT WHEN PUT AND PULL THE HOLDER.
//THIS VALUE AFECTS DISTANCE TO CLEAR THE SPINDLE FROM HOLDER                          
	if (MoveZ(HOLDER_Z, RapidMove))	// BAJA EL EJE Z PARA ALINEARSE AL CARRUSEL
		return 1;


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


//AFTER MATCHING HEIGHT ABOVE, APPROACH TOOL HOLDER BY MOVING TO FORK X POSITION

	if (MoveXY(ToolPositionX(CurrentTool), TOOL_CHANGE_SAFE_NEG_Y - HOLDER_Y, SlowSpeed))
		return 1;


	Delay_sec(1);

	//-if tool holder is not in the right position, operation will halt to avoid possible crash
	//while (!ReadBit(HOLDER_POSITION))
	//{
	///printf("tool holder is not in the right position\n");    
	//MsgBox("CRASH WARNING:\n TOOL HOLDER IS OUT OF POSITION\n CAN'T CONTINUE", MB_ICONHAND | MB_OK);
	//SetBit(56);
	//return 1;
	//}

	// - Read EMPTY HOLDER bit to see whether the tool holder is empty, to make a safe move without crashing other tool 
	//if (ReadBit(EMPTY_HOLDER))
	//{
	//printf("other tool in the way\n");
	//MsgBox("WARNING:\n OTHER TOOL IS IN THE WAY\n CAN'T CONTINUE", MB_ICONHAND | MB_OK);
	//SetBit(56);
	//return 1;


	//-if carousel cover did not open while tool change operation, will halt to avoid crash
	//while (!ReadBit(CAROUSEL_COVER))

	//{
	//printf("carousel cover did not open\n");  
	//MsgBox("WARNING:\n CAROUSEL COVER DID NOT OPEN\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))
	//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);

	// - 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;
	///}

	//-if carousel cover did not open while tool change operation, will halt to avoid crash
	//while (!ReadBit(CAROUSEL_COVER))

	//{
	//printf("carousel cover is not in open position\n");   
	//MsgBox("WARNING:\n CAROUSEL COVER IS NOT IN OPEN POSITION\n CAN'T CONTINUE",MB_ICONHAND | MB_OK);
	//SetBit(56);
	//return 1;
	//}


// - Move Z axis up at speed defined by 'Z_TOOL_RETRACT_SPEED', to Z_SAFE_HEIGHT

	if (MoveZ(HOLDER_Z + SAFE_HEIGHT_Z, TOOL_RETRACT_SPEED_Z))	// SUVE EL EJE Z PARA ROTAR EL CARRUSEL
		return 1;






	// - 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 x position of tool holder as a function of the tool
float ToolPositionX(int tool)
{
	return (HOLDER_X - HOLDER_X) * (tool - 1) + HOLDER_X;
}







//read binary code sensors to identify each tool
int CarouselPosition(void)
{
	int Tool = ReadBit(BITD) * 8 + ReadBit(BITC) * 4 + ReadBit(BITB) * 2 + ReadBit(BITA);

	return Tool;
}

// return Carosel Position when stable otherwise return invalid (-1)
int CarouselPositionStable(void)
{
	static int LastReadPosition=-1;
	static double LastChangeTime=0;
	int NewReadPosition;
	
	NewReadPosition = CarouselPosition(); //read sensors 
	
	if (NewReadPosition != LastReadPosition)  // changed?
	{
		LastChangeTime= Time_sec();  // record when
		LastReadPosition = NewReadPosition;
	}
	else
	{
		if (Time_sec() > LastChangeTime + 0.2) // no changes for a period of time?
			return NewReadPosition;  // return stable reading
	}
	return -1; // not stable return invalid
}





// 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 || (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);

	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)
		{
			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
}