//STEPCRAFT linear toolchanger code

// (1) STEPCRAFT port definition
int Chuckopenport = 1;
int Chuckopenpin = 14;

double[] ToolX = new double[11];
double[] ToolY = new double[11];
ToolX[0] = 0; // Tool0 X position
ToolY[0] = 400; // Tool0 Y position

// (2) STEPCRAFT tool holder XY positions
ToolX[1] = 546.9; // Tool1 X position
ToolY[1] = 805.1; // Tool1 Y position
ToolX[2] = 521.7; // Tool2 X position
ToolY[2] = 804.2; // Tool2 Y position
ToolX[3] = 496.8; // Tool3 X position
ToolY[3] = 804.3; // Tool3 Y position
ToolX[4] = 471.8; // Tool4 X position
ToolY[4] = 804.3; // Tool4 Y position
ToolX[5] = 446.9; // Tool5 X position
ToolY[5] = 804.3; // Tool5 Y position

// (3) STEPCRAFT optional tools
// ToolX[6] = 100; // Tool6 X position
// ToolY[6] = 180; // Tool6 Y position
// ToolX[7] = 100; // Tool7 X position
// ToolY[7] = 180; // Tool7 Y position
// ToolX[8] = 100; // Tool8 X position
// ToolY[8] = 180; // Tool8 Y position
// ToolX[9] = 100; // Tool9 X position
// ToolY[9] = 180; // Tool9 Y position
// ToolX[10] = 100; // Tool10 X position
// ToolY[10] = 180; // Tool10 Y position

// (4) STEPCRAFT basic parameters for tool change
double SafeZ = -2;
double Ztoolrelease = -76.2;
double Ztoolpickup = -76.2;
double Ytooloffset = -20; //STEPCRAFT tool position offset for Y
double Ztooloffset = -50; //STEPCRAFT tool position offset for Z

int Newtool = exec.Getnewtool();
int Currenttool = exec.Getcurrenttool();

if(Newtool == -1) // If new tool number is -1 means a missing T code, so we need to stop here...
return; 

// (5) STEPCRAFT max. number of tools possible
if(Newtool <0 || Newtool >5) // Tool number is out of range, so we need to stop here...
return;

if(Newtool == Currenttool) // Same tool was selected, so do nothing, stop here...
return; 

if(!exec.GetLED(56)||!exec.GetLED(57)||!exec.GetLED(58)) // If machine was not homed then it is unsafe to move in machine coordinates, stop here...
{
  MessageBox.Show("The machine was not yet homed, do homeing before executing a tool change!");
  exec.Stop();
  return;
}

while(exec.IsMoving()){}

// Get current XY machine coordinates to return to this position at the end of the macro

double Xoriginalpos = exec.GetXmachpos();
double Yoriginalpos = exec.GetYmachpos();

// Stop spindle if running and Move Z up

exec.Stopspin(); 
exec.Code("G00 G53 Z"+ SafeZ); // Move Z up
while(exec.IsMoving()){}

if(Currenttool!=0)
{
  // Move to old tool position on XY plane

  // STEPCRAFT
  exec.Code("G00 G53 X" + ToolX[Currenttool] + " Y" + (ToolY[Currenttool] +Ytooloffset));
  while(exec.IsMoving()){}

  // Drop current tool
  exec.Code("G00 G53 Z"+ Ztoolrelease); // Move Z axis down to tool holder position

  // STEPCRAFT
  exec.Code("G01 F500 G53 Y" + ToolY[Currenttool]); // Move Y axis to tool holder position

  while(exec.IsMoving()){}
  exec.Setoutpin(Chuckopenport, Chuckopenpin); // Open the chuck with pneumatic valve
  exec.Wait(1000); // Wait one 1000msec
  
  // STEPCRAFT
  exec.Code("G01 F500 G53 Z"+ (SafeZ + Ztooloffset)); // STEPCRAFT Move Z up for tool change
  while(exec.IsMoving()){}
}

if(Currenttool==0)
{

  // STEPCRAFT open chuck if tool = 0
  exec.Wait(200);
  MessageBox.Show("No tool in chuck. Correct?"); // STEPCRAFT safety question
  exec.Setoutpin(Chuckopenport, Chuckopenpin); // Open the chuck with pneumatic valve
  exec.Wait(1000); // Wait one 1000msec
}

if(Newtool!=0)
{
  // Move to new tool position on XY plane
  exec.Code("G00 G53 X" + ToolX[Newtool] + " Y" + ToolY[Newtool]);
  while(exec.IsMoving()){}

  // Pick new tool
  exec.Code("G01 F1000 G53 Z"+ Ztoolpickup); // Move Z axis down to tool holder position

  while(exec.IsMoving()){}
  exec.Clroutpin(Chuckopenport, Chuckopenpin); // Close the chuck with pneumatic valve
  exec.Wait(1000); // Wait one 1000msec

  while(exec.IsMoving()){}
  if(!exec.Ismacrostopped()) // If tool change was not interrupted with a stop only then validate new tool number
  {
  exec.Setcurrenttool(Newtool); //Set the current tool -> the new tool
  //MessageBox.Show("Tool change done."); // STEPCRAFT no message required
  }

  // STEPCRAFT
  exec.Code("G01 F1000 G53 Y" + (ToolY[Newtool] +Ytooloffset)); // Move Y axis to tool holder offset position

  exec.Code("G00 G53 Z"+ SafeZ); // Move Z up
  while(exec.IsMoving()){}
}

if(Newtool==0)
{
  // Move Z/axis up with open chuck
  exec.Code("G00 G53 Z"+ SafeZ); // Move Z up

  while(exec.IsMoving()){}
  exec.Clroutpin(Chuckopenport, Chuckopenpin); // Close the chuck with pneumatic valve
  exec.Wait(1000); // Wait one 1000msec

  while(exec.IsMoving()){}
  if(!exec.Ismacrostopped()) // If tool change was not interrupted with a stop only then validate new tool number
  {
  exec.Setcurrenttool(Newtool); //Set the current tool -> the new tool
  //MessageBox.Show("Tool change done."); // STEPCRAFT no message required
  }
  while(exec.IsMoving()){}
}



// Move back to start point

exec.Code("G00 G53 X" + Xoriginalpos + " Y" + Yoriginalpos);
while(exec.IsMoving()){}

// Measure new tool will go here....
// Getting the tool offset of the new tool
exec.Code("G43 H"+Newtool); // Load new tool offset

exec.Wait(200);

while(exec.IsMoving()){}
if(exec.Ismacrostopped()) // STEPCRAFT interruption
{
  exec.StopWithDeccel();
  MessageBox.Show("Tool change was interrupted by user!");
}