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


// Tool Touch Position
#define YTouch 1
#define XTouch 9.5
#define StepsIN 2539.979	//X Y Racks Steps per Inch		
#define ZStepsIN 4064		//Z Axis Ball Screw


#define Fast 2500
#define Slow 50

main()
{

	double T1, T2,AxisOffsetZ;
	
		//Move the tool over the tool height gauge
		Move(7,0);
		while(!CheckDone(7));
		
		
		MoveAtVel(6,XTouch*StepsIN,9000); 
		MoveAtVel(5,YTouch*StepsIN,9000); 
		while(!CheckDone(5)||!CheckDone(6));
		
		
		// Axis 1 Measurement
		Jog(7,-Fast);  			// jog slowly negative
		while(!ReadBit(TouchFlag)) ;  // loop until IO bit goes high
		Jog(7,Slow);				//Reverse Direction 
		while(ReadBit(TouchFlag)); 
		Jog(7,0);				// stop
		while(!CheckDone(7)) ; // loop until motion completes 
		
		T1 = ch7->Dest;	//Record Current Position
		
		Move(7,-10); //Retract Axis
		while (!CheckDone(7));
		
		printf("T1 = %g\n",T1/ZStepsIN);

		//Calculate Axis Position
 //       float NewZOffset=(ZTouch-ZTouch)*-1;
        

		
//Change Tools
	int Answer;

	Answer = MsgBox("Have You Changed Tools?",MB_YESNO|MB_ICONEXCLAMATION);
	if (Answer == IDYES)
	{
		// Axis 1 Measurement
		Jog(7,-Fast);  			// jog slowly negative
		while(!ReadBit(TouchFlag)) ;  // loop until IO bit goes high
		Jog(7,Slow);				//Reverse Direction 
		while(ReadBit(TouchFlag)); 
		Jog(7,0);				// stop
		while(!CheckDone(7)) ; // loop until motion completes 
		
		T2 = ch7->Dest;	//Record Current Position
		
		Move(7,-10); //Retract Axis
		while (!CheckDone(7));
		
		printf("T2 = %g\n",T2/ZStepsIN);

//Calculate new values
		printf("Tool Length Change = %g\n",(T2-T1)/ZStepsIN);

//Update Axis Offset		
	
	GetAxisOffset(&AxisOffsetZ, 2);		//0=X, 1=Y, 2=Z
	printf("Axis Offset Z = %g\n",AxisOffsetZ);
	
	//Set New Z Position
	
//	ch7->Dest=(ch7->(T2-T1));
	


	}

	else
	{
		return;
	}		
		
		
	
}


// Set Z DRO to specified value by adjusting current Fixture Offset
// Similar functionality as Set button but will function while Job is running
#define Zaxis 2
void SetFixZ(double value)
{
    int FixtureIndex;
    double NewOriginOffset,OriginOffsetZ;
    double DROx, DROy, DROz, DROa, DROb, DROc;

    GetFixtureIndex(&FixtureIndex);

    GetOriginOffset(&OriginOffsetZ, FixtureIndex, Zaxis);

    GetDROs(&DROx, &DROy, &DROz, &DROa, &DROb, &DROc);
    
    // Adjust Origin Offset to make DRO the value
//    NewOriginOffset = OriginOffsetZ + DROz - value;
    NewOriginOffset = value;
    
    SetUserDataDouble(TMP,NewOriginOffset);
    SetVars(5201+FixtureIndex*20+Zaxis, 1, TMP);

    DoPC(PC_COMM_UPDATE_FIXTURE);
}