//M32 X Probing

double probeX = 200;
double probeY = 300;
double Xmax = 200;
double Zmin = -100;
double FeedrateFast = 400;
double FeedrateSlow = 50;
double SafeZ = 100;
double retractheight = 10;
double retractforsecondmeasurement = 2;
double Zback = 5;
double probeOffsetZ = 39.74;
string ActiveWorkZ = AS3.Getfield(899);
string fixture = ActiveWorkZ.Replace("Active fixture: G", "");

string ActiveWorkZString = fixture.ToString(); //Konvertera till string
exec.Writekey("JB CNC", "ActiveWorkZ", ActiveWorkZString); //Skriv till profil
double ActiveWorkZDouble = 0;
ActiveWorkZDouble = Convert.ToDouble(fixture);

double SetFeedRate = 0; //Ändra ej

bool domoveXY = false; //Enable XY movement
bool dodualcycle = true; //Do probing from 2 cycles, first with Fast and second with Slow feedrates

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, home the machine before run to parking position!");
  exec.Stop();
  return;
}

while(exec.IsMoving()){}

double Zoriginalpos = exec.GetZpos(); // Get the current machine coordinates (med offset ej G53)
//double Yoriginalpos = exec.GetYmachpos(); // Get the current machine coordinates 

Double ProbeDistance = Zmin + Zoriginalpos;
//AS3.Additemtolist("Max pos = " + ProbeDistance ,2);

if(dodualcycle)
{
  exec.Code("G31 Z" + ProbeDistance + "F" + FeedrateFast); // Do the Z probing with Fast feedrate first
  while(exec.IsMoving()){}

  exec.Code("G91 G0 Z" + retractforsecondmeasurement);
  exec.Code("G90"); 
}

while(exec.IsMoving()){}
exec.Code("G31 Z" + ProbeDistance + "F" + FeedrateSlow); // Do the Z probing again with Slow Feedrate to get a more accurate reading
while(exec.IsMoving()){}

double FinnishPos = exec.GetZpos(); // Get the current machine coordinates
double FinnishProbeDistance = FinnishPos - Zoriginalpos;

//AS3.Additemtolist("Probe pos = " + FinnishPos ,2);
//AS3.Additemtolist("Probe distance =  " + FinnishProbeDistance,2);
if(FinnishPos <= Zoriginalpos - Xmax ){
	MessageBox.Show("Probe did not touch the material!");
	exec.Code("F" + SetFeedRate);
	exec.Stop();
}
else
{
	if (ActiveWorkZDouble == 54)
	{
    AS3.Setfield(probeOffsetZ, 99); // Sätter värdet
    AS3.Validatefield(99);          // Validerar
	}

	if (ActiveWorkZDouble == 55)
	{
    AS3.Setfield(probeOffsetZ, 105); // Sätter värdet
    AS3.Validatefield(105);          // Validerar
	}
    
	if (ActiveWorkZDouble == 56)
	{
    AS3.Setfield(probeOffsetZ, 111); // Sätter värdet
    AS3.Validatefield(111);          // Validerar
	}	

	if (ActiveWorkZDouble == 57)
	{
    AS3.Setfield(probeOffsetZ, 117); // Sätter värdet
    AS3.Validatefield(117);          // Validerar
	}	

	if (ActiveWorkZDouble == 58)
	{
    AS3.Setfield(probeOffsetZ, 123); // Sätter värdet
    AS3.Validatefield(123);          // Validerar
	}		
	
	if (ActiveWorkZDouble == 59)
	{
    AS3.Setfield(probeOffsetZ, 129); // Sätter värdet
    AS3.Validatefield(129);          // Validerar
	}	
	
	exec.Code("G91 G0 Z" + Zback);
    exec.Code("F" + SetFeedRate);
    exec.Code("G90");
}