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

//-----------------------------------------
//		LINEAR TOOL CHANGING
//-----------------------------------------
#define AXISX 0
#define AXISY 1
#define AXISZ 2

//---------Absolute position of tool holders
#define HOLDER_Y_1 902.8
#define HOLDER_Y_2 823.2
#define HOLDER_Y_3 743.85
#define HOLDER_Y_4 664.5
#define HOLDER_Y_5 585.15
#define HOLDER_Y_6 505.8
#define HOLDER_Y_7 426.45
#define HOLDER_Y_8 347.1
#define HOLDER_Y_9 267.75
#define HOLDER_Y_10 188.4
#define HOLDER_Y_11 109.05
#define HOLDER_Y_12 29

#define HOLDER_X 1194
#define HOLDER_Z -132.1// absolute position of the tool fork holders
#define TOOL_HEIGHT_PLATE_X 996.64 
#define TOOL_HEIGHT_PLATE_Y 972.91 

// absolute position to move to that is permanently unobstructed, and safe to move down in Z
#define TOOL_CHANGE_SAFE_POS_X 1120  
#define TOOL_CHANGE_SAFE_POS_Y 902.55
#define CLEAR_ADJACENT_TOOLS_POS_X 1142

#define AXIS_SAFE_DISTANCE_X 80// distance in mm to approach tool holder
//---------

//--------- Spindle IO bits
#define CLAW_EJECT 48		// IO bit to eject tool from spindle (KONNECT OUTPUT 0)
#define CASE_PRESSURE 49	// IO bit to apply 10 psi spindle case pressure (KONNECT OUTPUT 1)
#define SPINDLE_CLEAN 50	// IO bit to blow air out of spindle taper (KONNECT OUTPUT 2)
//#define CLAW_LOOSE 1048		// IO bit to sense whether the claw has ejected (KONNECT INPUT 24)
//#define TOOL_SENSE 1049		// IO bit to sense whether the a tool is in the spindle (KONNECT INPUT 24)
//---------

#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-12 Tool Slot loaded into Spindle
#define TOOL_DISK_FILE "c:\\Temp\\ToolChangerData.txt"


#define CLAMP_TIME 1.0    	// seconds to wait for the clamp/unclamp
#define BLAST_TIME 1.0    	// seconds to wait for the taper blast
#define RELEASE_TIME 0.025	// seconds to wait after eject before moving
#define TOOL_HEIGHT_BIT	1055	//bit to read tool height plate (KONNECT INPUT 31)




#define SAFE_HEIGHT_Z 85   // relative distance in mm to move to clear the top of the tool taper
#define TOOL_RETRACT_SPEED_Z 80	//speed in mm/second to move spindle up after tool has been ejected
#define EJECT_DISTANCE_Z 8	//distance in mm to move spindle UP as tool is being ejected
#define EJECT_SPEED_Z 40	//speed in mm/second to move spindle UP as tool is being ejected
#define ENGAGE_DISTANCE_Z 4	//distance in mm to move spindle DOWN as tool is being engaged
#define ENGAGE_SPEED_Z 90	//speed in mm/second to move spindle DOWN as tool is being engaged

#define SLOWSPEED		30.0 //mm/sec
#define MEDIUMSPEED		200.0 //mm/sec
#define MED_SPEED_Z		60.0 //mm/s
#define CNT_PER_MM_X 160.3862 
#define CNT_PER_MM_Y 160.3272 
#define CNT_PER_MM_Z 629.921

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



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)
{
	// Stop spindle
	ClearBit(0);
	ClearBit(1);	
	
	int CurrentTool;

	if (GetCurrentTool(&CurrentTool)) return 1;  //  -1=Spindle empty, 0=unknown, 1-12 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!=-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(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 Tool)
{
	
	// - Move to position of requested tool
	// - Rapid move to absolute position of new tool only in X and Y
	if (MoveXY(HOLDER_X,ToolPositionY(Tool),MEDIUMSPEED)) return 1;
	
	// Check CLAW_EJECT bit is high, if not turn on.
	
	if (!ReadBit(CLAW_EJECT))
		{
			SetBit(CLAW_EJECT);
		}

	// - Move to tool Z position + engage distance at SLOWSPEED
	if (MoveZ(HOLDER_Z+ENGAGE_DISTANCE_Z,SLOWSPEED)) return 1;

	// - Taper blast before engaging new tool
	ClearBit(CASE_PRESSURE); //Turn off case pressure	
	SetBit(SPINDLE_CLEAN);
	
	Delay_sec(BLAST_TIME);
	
	ClearBit(SPINDLE_CLEAN); // turn off taper blast before engaging new tool
	SetBit(CASE_PRESSURE); // Turn ON case pressure

	// - Engage new tool
	// - Turn off CLAW_EJECT  to engage tool
	ClearBit(CLAW_EJECT);
	// - Move Z axis DOWN at speed to match engaging of tool
	if (MoveZ(HOLDER_Z,ENGAGE_SPEED_Z)) return 1;

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

	// - 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("Claw Still Loose Error\n");
		MsgBox("Claw Still Loose Error\n", MB_ICONHAND | MB_OK);
		return 1;
	}
	if (!ReadBit(TOOL_SENSE))
	{ 
		printf("Tool Sense Error\n");
		MsgBox("Tool Sense Error\n", MB_ICONHAND | MB_OK);
		return 1;
	}
*/
	// - Leave tool holder by moving X axis to CLEAR_ADJACENT_TOOLS_POS_X
	if (MoveXY(CLEAR_ADJACENT_TOOLS_POS_X,ToolPositionY(Tool),SLOWSPEED)) return 1;

	// - Rapid to Z home
	if (MoveZ(0.0,MED_SPEED_Z)) return 1;
	
	// - Move to safe X,Y position
	if (MoveXY(TOOL_CHANGE_SAFE_POS_X,ToolPositionY(Tool),MEDIUMSPEED)) return 1;
	if (MoveXY(TOOL_CHANGE_SAFE_POS_X,TOOL_CHANGE_SAFE_POS_Y,MEDIUMSPEED)) return 1;

// - Move to tool height plate X,Y position
	if (MoveXY(TOOL_HEIGHT_PLATE_X,TOOL_HEIGHT_PLATE_Y,MEDIUMSPEED)) return 1;

	return 0; //success
}


// - Remove tool in spindle by going to holder of current tool
int UnloadTool(int CurrentTool)
{
	// - Rapid to Z Home to clear any work that may be on the table
	if (MoveZ(0.0,SLOWSPEED)) return 1;
	
	// - Rapid to TOOL_CHANGE_SAFE_POS
	if (MoveXY(TOOL_CHANGE_SAFE_POS_X,TOOL_CHANGE_SAFE_POS_Y,MEDIUMSPEED)) return 1;
	
	// - Move to holder Y position
	if (MoveXY(TOOL_CHANGE_SAFE_POS_X,ToolPositionY(CurrentTool),MEDIUMSPEED)) return 1;
	
	// - Approach tool slot X position
	if (MoveXY(CLEAR_ADJACENT_TOOLS_POS_X,ToolPositionY(CurrentTool),SLOWSPEED)) return 1;
	
	// - Approach tool holder by matching Z height of tool flange currently in spindle with tool holder claw
	if (MoveZ(HOLDER_Z,SLOWSPEED)) return 1;

	// - After matching Y position, match tool X position
	if (MoveXY(HOLDER_X,ToolPositionY(CurrentTool),SLOWSPEED)) return 1;

	// - Move only in X 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);
	
	// - wait for release
	Delay_sec(RELEASE_TIME);
	
	// - Move Z axis up at speed to match ejection of tool
	if (MoveZ(HOLDER_Z+EJECT_DISTANCE_Z,EJECT_SPEED_Z)) return 1;

	// - Turn on SPINDLE_CLEAN bit to remove any debris from taper and tools
	ClearBit(CASE_PRESSURE); //Turn off case pressure	
	SetBit(SPINDLE_CLEAN); // Taper blast

	// - Wait for time in seconds defined by CLAMP_TIME
	Delay_sec(BLAST_TIME);
	
	// - Turn off SPINDLE_CLEAN bit
	ClearBit(SPINDLE_CLEAN);
	SetBit(CASE_PRESSURE); //Turn on case pressure
	
	// - 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");
		//MsgBox("Claw Loose Error\n", MB_ICONHAND | MB_OK);
		//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)) 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");
		//MsgBox("Tool Sense Release Error\n", MB_ICONHAND | MB_OK);
		//return 1;
	//}
	return 0; // success
}



//return y position of tool holder as a function of the tool
float ToolPositionY(int tool)
{
	return (HOLDER_Y_2-HOLDER_Y_1)*(tool-1) + HOLDER_Y_1;
}



// 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-12 = valid tool
BOOL ToolNumberValid(int tool)
{
	return tool == -1 || (tool>=1 && tool<=12);
}


// 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_MM_X, Speed * CNT_PER_MM_X);
	MoveAtVel(AXISY, y * CNT_PER_MM_Y, Speed * CNT_PER_MM_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_MM_Z, Speed * CNT_PER_MM_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
}